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1.0  INTRODUCTION 


This  report,  prepared  by  the  Systems  Simulation  and  Technology  Division,  of 
the  Tank-Automotive  Technology  Directorate  of  the  U.  S.  Army  Tank-Automotive 
Command  (TACOM) ,  describes  an  analytical  study  of  the  dynamics  and  kinematics 
of  a  Turret  Motion  Base  Simulator  (TMBS)  .  The  TMBS  is  a  six  degree-of- 
freedom  motion  base  simulator  being  developed  for  TACOM.  The  TMBS  is 
described  as  a  Stewart  platform  which  consists  of  a  base  which  is  fixed  to 
the  ground,  and  an  upper,  or  driven  platform.  As  shown  in  Figure  5-1,  the 
movable  platform  of  the  TMBS  to  which  the  turret  is  attached,  is  driven  by  6 
linear  hydraulic  actuators.  Each  actuator  is  attached  between  the  fixed  base 
and  the  moveable  platform  and  is  mounted  at  a  different  angle  in  space  to 
deliver  six  independent  motions  to  the  platform.  The  basis  of  the  Stewart 
platform  was  developed  for  flight  simulators  and  is  described  in  reference  1. 
Many  types  of  Stewart  platforms  have  been  developed  for  laboratory 
simulation.  However,  what  makes  the  TMBS  unique  is  the  design  capability  to 
handle  up  to  a  25-ton  load  with  a  goal  to  achieve  up  to  a  10  Hz  bandwidth 
motion . 

This  report  covers  the  dynamic  and  kinematic  study  of  the  TMBS,  that  is,  the 
mathematical  analysis  to  describe  the  actuator  and  platform  motions.  This 
portion  of  the  study  consists  of  an  analytical  investigation  utilizing  and 
developing  computer  software.  Several  issues  will  be  mentioned  regarding  the 
incorporation  of  the  dynamic/kinematic  model  with  the  hydraulic  control  model 
but  this  area  is  considered  the  next  stage  of  the  analysis  and  will  be 
addressed  in  more  detail  at  a  later  time.  The  goal  is  to  develop  a  complete 
comprehensive  model  which  will  be  used  for  the  dynamic/kinematic  analysis 
described  in  this  study,  along  with  the  entire  hydraulic  and  control  systems 
of  the  TMBS.  This  report  documents  the  FORTRAN  software  which  has  been 
developed  to  conduct  analysis  on  the  TMBS  dynamics  and  kinematics.  This 
software  was  written  in  subroutine/modular  form  so  that  it  can  be 
conveniently  imported  to  a  comprehensive  model  and  other  applications.  A 
portion  of  the  software  developed  in  this  study  was  used  recently  in  support 
of  a  kinematic  animation  study  of  the  TMBS  which  is  shown  in  Figure  5-1. 

The  future  goal  is  to  develop  a  complete  comprehensive  model  which  will  be 
used  to  evaluate  and  conduct  analysis  on  various  TMBS  test  scenarios  before 
being  applied  to  the  real  system.  In  addition,  the  comprehensive  model  may 
be  used  to  determine  if  a  particular  test  specimen  (vehicle  subsystem, 
turret,  etc.)  is  compatible  with  the  TMBS  control  system.  Low  resonant 
natural  frequencies  from  a  test  specimen  could  hamper  the  overall  stability 
of  the  TMBS  control  system.  Further  analysis  with  the  comprehensive  model 
could  provide  impact  studies  and  insight  with  the  potential  to  derive  control 
design  modifications  to  maintain  proper  TMBS  operations.  Furthermore  the 
comprehensive  model  could  be  used  to  conduct  analysis  on  advanced  techniques 
and  strategies  for  man-in-the-loop  testing,  which  is  one  of  the  applications 
planned  for  the  TMBS. 


2.0  OBJECTIVE 

The  objective  of  this  portion  of  the  study  is  to  develop  a  means  of 
evaluating  the  dynamics  and  kinematics  of  the  TMBS  which  consists  of  the 
various  motions  of  the  platform  and  six  actuators  and  their  mathematical 
relationships.  This  report  contains  FORTRAN  software  developed  for  this 
study  which  is  listed  in  the  appendices  for  each  task.  In  addition  a  time 
domain  simulation  was  conducted  to  simulate  the  platform/actuator  motion  for 
a  given  set  of  actuator  forces.  The  simulation  was  conducted  using  Advanced 
Continuous  Simulation  Language  (ACSL)  and  the  FORTRAN  subroutines  mentioned 
above  which  are  listed  in  Appendix  C  and  D  respectively.  As  mentioned  above, 
the  objective  is  primarily  to  build  the  blocks  for  a  comprehensive 
mathematical  model  of  the  entire  TMBS  system.  This  would  include  modeling 
the  operation  of  the  TMBS.  Since  the  foundation  of  the  TMBS  operation  is 


based  on  classical  dynamics,  the  technique  chosen  for  this  study  employs 
Newton/Euler  equations  based  partly  on  technical  notes  received  from 
Contraves  Goertz  Corporation,  the  contractor  for  the  development  and  design 
of  the  TMBS.  Although  the  System  Simulation  and  Technology  Division  of  TACOM 
is  conducting  research  in  the  area  of  more  efficient  numerical  techniques  for 
evaluating  dynamics  and  kinematics,  this  analysis  is  based  on  Euler  angle 
transformations  due  to  the  nature  of  the  TMBS.  The  TMBS  will  be  driven  by 
command  signals  which  are  representative  of  Euler  angles.  In  addition,  part 
of  the  TMBS  control  monitor  system  will  be  based  on  Euler  angles. 

3.0  CONCLUSIONS 

As  shown  in  this  report  the  results  produced  by  this  study  were  compared  with 
results  of  a  Dynamic  Analysis  and  Design  Systems  (DADS)  simulation.  DADS  was 
jointly  developed  by  the  University  of  Iowa  and  TACOM  and  is  well  known  in 
the  area  of  dynamic  and  kinematic  system  modeling.  The  results  for  the  time- 
based  simulation  are  comparable  for  each  state  of  the  TMBS  up  to  the  2nd  and 
3rd  decimal  place.  It  is  concluded  that  the  study  presented  in  this  report 
is  fundamentally  correct.  The  difference  between  the  DADS  and  ACSL/FORTRAN 
model  results  can  be  considered  the  difference  in  numerical  techniques  of 
integration  and  precision. 

4.0  RECOMMENDATIONS 

There  appears  to  be  problems  when  the  model  of  this  study  is  combined  with 
the  hydraulic  control  model  for  the  TMBS.  There  seems  to  be  instability  in 
the  model  results  when  the  hydraulic  actuator  model  is  combined  with  the 
model  presented  in  this  report.  The  problem,  at  first,  was  believed  to  be  a 
numerical  instability  caused  by  insufficient  precision,  since  ACSL  is 
governed  by  single  precision.  A  further  investigation  is  being  made  at  this 
time.  The  results  may  show  an  inadequate  control  system  proposed  for  the 
TMBS.  The  entire  model  has  been  rewritten  in  ADSIM  (Applied  Dynamics 
Simulation  Language)  for  evaluation  on  a  AD100  computer  system  which  will  be 
used  for  other  TMBS  applications. 

5.0  DISCUSSION 

Each  portion  of  the  analysis  covered  in  this  report  will  be  supported  by 
FORTRAN  software  which  consist  of  a  program  called  ' TMBS_KINEMATICS'  listed 
in  Appendix  B  and  the  subroutines  used  are  listed  in  Appendix  D.  Appendix  A 
includes  a  listing  of  all  variables  used  in  both  the  equations  listed  in  this 
report  and  in  the  FORTRAN  listings.  This  will  make  it  convenient  to  cross 
reference  the  variables  from  equation  form  to  FORTRAN  code.  The  program 
TMBS_KINEMATIC  is  broken  down  to  the  tasks  required  to  evaluate  the  TMBS 
kinematics  and  is  menu  driven  so  that  a  task  can  easily  be  selected.  The 
tasks  offered  in  the  menu  are  detailed  in  table  5-1. 


TABLE  5-1  Task3  Used  in  the  KINEMATIC  Program 

Option/Task 

1.  Given  the  six  degrees  of  freedom  of  the  platform. 
What  are  the  six  corresponding  actuator  lengths? 

2.  Given  the  six  degrees-of-f reedom  of  the  platform  and 
the  magnitude  of  the  six  actuator  forces. 

What  are  the  net  torques  and  forces  on  the  platform? 

3.  Given  the  six-degrees-of-freedom  of  the  platform  and 
the  six  degrees  of  freedom  platform  rates? 

What  are  the  magnitudes  of  the  six  actuator  rates? 


4. 


Given  the  six  degrees  of  freedom  of  the  platform. 

What  is  the  Jacobian  Matrix  (S  matrix)  and  Decoupling 
Matrix  (D  matrix) ? 

In  addition,  when  given  the  S  matrix  and  when: 

A.  Given  the  magnitude  of  the  six  actuator  rates. 

What  are  the  six-degree-of-freedom  platform  rates? 

(Inverse  of  option  3) 

B.  Given  the  six-degree-of-freedom  platform  rates. 

What  is  the  magnitude  of  the  six  actuator  rates? 

(Different  method  but  produces  same  results  as  option  3) 

5.  Given  the  six  actuator  lengths. 

What  are  the  corresponding  six  degrees  of  freedom  of  the  platform? 
(Inverse  of  option  1) 

These  tasks  represent  the  minimum  capabilities  required  to  produce  a  dynamic 
kinematic  model  of  the  TMBS .  Further  tasks  may  have  to  be  established  for 
the  complete  comprehensive  model.  The  TMBS  KINEMATIC  program  was  the  initial 
step  of  this  study,  which  consisted  of  having  each  task  produce  the  correct 
results  from  the  FORTRAN  program  and  subroutines.  The  next  step  was  the 
development  of  a  time-base  simulation  of  the  TMBS  by  importing  the  FORTRAN 
subroutines  into  a  simulation  language  which  has  sorting  and  integration 
process  capabilities .  Although  other  forms  of  software  could  have  been  used 
at  this  point,  the  final  goal  once  again,  was  to  have  simulation  capability 
for  the  complete  comprehensive  model.  The  simulation  language  available  for 
this  study  was  the  Advanced  Continuous  Simulation  Language  (ACSL)  which  was 
used  to  evaluate  time  simulation  of  the  TMBS  dynamics.  The  listing  is  shown 
in  Appendix  C  which  uses  some  of  the  subroutines  from  Appendix  D  in  the 
procedurals . 

The  rest  of  this  report  documents  the  methodologies  used  to  develop  these 
analytical  tools  along  with  some  of  the  results  produced  from  the  FORTRAN 
program. 

5.1  TMBS  COORDINATE  CONFIGURATION 


Shown  in  Figure  5-2  is  a  drawing  of  the  TMBS  system  that  illustrates  the 
coordinate  systems  and  vector  configurations.  Although  this  drawing  is  a 
rather  simplistic  view  of  the  TMBS  system,  it  will  be  sufficient  for  the 
mathematical  representation.  The  two  disks  shown  represent  the  platform  and 
base  of  the  TMBS.  The  platform  and  base  are  joined  by  the  actuator  vectors 
(LI,  L2,  L3, L4,  L5,  and  L6)  and  the  P  vector.  The  platform  is  free  to  move  in 
the  six  degrees  of  freedom  described  by  three  translational  and  three 
rotational  motions,  while  the  base  is  fixed  to  ground.  The  two  coordinate 
systems  shown  represent  the  platform  coordinates  (body  coordinates)  and  base 
coordinates  (global  coordinates)  which  are  fixed  to  the  two  bodies 
respectively.  Vectors  will  be  associated  with  these  coordinate  systems  by 
the  subscript  "p"  and  "b"  respectively.  The  actuators  are  connecting  the 
base  and  platform  by  means  of  swivel  joints  which  are  free  to  rotate  around 
the  swivel  axes  within  reasonable  limits  The  swivel  joints  are  located  in 
the  figure  by  each  arrow  of  the  platform  vectors  and  base  vectors.  The 
platform  is  described  by  six  platform  vectors  (Ppl,  Pp2,  Pp3,  Pp4,  Pp5  &  Pp6) 
which  are  fixed  to  the  platform  and  connect  the  platform  coordinate  system  to 
the  platform  swivel  joints.  The  base  vectors  (Bbl,  Bb2,  Bb3,  Bb4,  Bb5  &  Bb6) 
are  configured  in  a  similar  fashion  between  the  base  coordinate  system  and 
the  base  swivel  joints.  As  shown,  the  vector,  which  represents  the  actuator 
(LI,  L2 ,  L3,  L4,  L5  &  L6)  in  three-dimensional  space,  connects  the  swivel 
joints  of  the  platform  to  the  swivel  joints  of  the  base.  Each  platform 
vector  (Ppi) ,  base  vector  (Bbi) ,  and  actuator  vector  (Li)  are  associated  with 


Figure  5-2-  Vector  Coordinate  Configuration 
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an  index  subscript  "i"  (i=l,2, 3, 4, 5, 6)  .  The  P  vector  connects  the  two 
coordinate  systems  and  basically  describes  the  position  between  the  two 
coordinate  systems.  The  base  and  platform  vectors  are  better  illustrated  in 
Figure  5-3,  which  shows  the  geometry  from  a  top  view.  This  is  the  coordinate 
configuration  used  for  this  study.  Table  3  shows  the  values  for  the  platform 
and  base  vectors .  These  values  are  produced  from  the  subroutine  "CONFIG" . 
(See  Appendix  D) 

Table  1  CONFIGURATION  VECTORS  Bbi  &  Ppi 


Base  Vectors  Bbi 


Actuator  (1) 

X 

Y 

Z 

1 

124.68 

-9.00 

0.0 

2 

-54.54 

-112.47 

0.0 

3 

-70.13 

-103.47 

0.0 

4 

-70.13 

103.47 

0.0 

5 

-54.54 

112.47 

0.0 

6 

124.68 

9.00 

0.0 

Platform  Vectors  Ppi 

Actuator(i)  X 

Y 

Z 

1 

57.59 

-81.75 

0.0 

2 

42.00 

-90.75 

0.0 

3 

-99.59 

-9.0 

0.0 

4 

-99.59 

9.0 

0.0 

5 

42.00 

90.75 

0.0 

6 

57.59 

81.75 

0.0 

5.2  EULER  TRANSFORMATION  MATRIX 


The  Euler  angle  configuration  for  this  study  consists  of  a  rotation  about  the 
Z  axis,  then  the  resultant  axis  is  rotated  about  the  y  axis,  and  finally  the 
resultant  axis  is  rotated  about  the  x  axis.  The  Euler  angles  will  be  denoted 
as  EUz,  EUy,  and  EUx  respectively.  The  transformations  for  the  individual 
rotations  are  defined  as  follows: 


1. 

0. 

0. 

Rx (EUx)  » 

0. 

Cos (EUx) 

-Sin  (EUx) 

0. 

Sin (EUx) 

Cos (EUx) 

Cos (EUy) 

0. 

Sin (EUy) 

Ry (EUy)  « 

0. 

1. 

0. 

-Sin  (EUy) 

0. 

Cos (EUy) 

Cos (EUz) 

-Sin  (EUz) 

0. 

Rz  (EUz)  - 

Sin (EUz) 

Cos (EUz) 

0. 

0. 

0. 

1. 

(Eg  1) 


(Eg  2) 


(Eg  3) 


* 


The  resulting  rotation  matrix  (transformation  matrix)  is  defined  as  the 
product  of  the  individual  matrices  as  follows: 
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R  =  Rz(EUz)  Ry(EUy)  Rx(EUx)  =  Rzyx (EUz, EUy, EUx) 


Cos (EUz)Cos (EUy)  -Sin (EUz) Cos (EUx)  +  Sin (EUy) Cos (EUz) Sin (EUx) 


Sin  (EUz) Sin (EUx)  ♦  Sin (EUy) Cos (EUz) Cos (EUx) 


(Eq  4) 


Cos (EUy) Sin (EUz)  Cos (EU2) Cos (EUx)  +  Sin (EUz) Sin (EUy) Sin (EUx) 

-Sin (EUy)  Cos (EUy) Sin (EUx) 


-Cos (EUz) Sin (EUx)  +  Sin (EUz) Sin (EUy) Cos (EUx) 

Cos (EUy) Cos (EUx) 


This  transformation  matrix  will  be  used  to  transform  vectors  from  platform 
coordinates  to  base  coordinates,  and  it  will  be  known  as  the  forward 
transformation  matrix.  The  reverse  transformation  matrix  produces  the 
opposite  transformation  by  converting  vectors  from  base  coordinates  to 
platform  coordinates.  The  reverse  transformation  is  the  inverse  of  the 
forward  transformation  which  is  reduced  to  the  transpose  of  the  forward 
transformation  matrix  due  to  orthogonal  matrix  properties  (See  reference  5 
pp.  336) .  It  should  be  mentioned  that  there  are  singularity  problems  when 
using  these  transformation  matrices.  However,  for  the  range  of  Euler  angles 
used  for  the  TMBS  (EUx,  EUy,  EUz  <  90  deg.),  this  will  not  present  a  problem. 
Both  of  these  transformation  matrices  are  computed  in  subroutines  "TRANS_MAT" 
and  "R_TRANS_MAT",  respectively.  (See  Appendix  D) 

Rxyz  =  Rzyx"1  =  Rzyx1  (Eq  5) 

From  now  on  in  this  report,  the  transformation  notation  will  be: 

Forward  transformation 

(Rxyz)  is  denoted  as:  pRj,  (Platform  to  Base  coordinates) 

Reverse  transformation 

(Rzyx)  is  denoted  as:  bR,,  (Base  to  Platform  coordinates) 

For  example  a  vector  ' V'  in  platform  coordinates  'Vp'  could  be  transformed  to 
base  coordinates  by  vb  =  PR,,  *  Vp  or  denoted  as  Vb  =  (pRi,  *  Vp)b  where  pRb  is  a 
3X3  matrix  and  vector  Vp  can  be  considered  a  3X1  matrix  causing  the  resultant 
product  Vb  to  be  a  3X1  matrix  (vector)  . 


5.3  POSITION  FORWARD  KINEMATICS 

Now  that  the  tools  are  in  hand,  the  first  task  can  be  accomplished,  which  is 
to  determine  the  actuator  length  by  knowing  the  platform  orientation.  Shown 
in  Figure  5-4  is  a  drawing  representing  the  position  vectors  of  the  TMBS. 
This  drawing  is  a  simplified  case  of  Figure  5-2,  where  the  vectors  are 
reduced  to  one  set  for  one  general  actuator/swivel  joint.  The  index  "i"  will 
be  used  to  specify  which  of  the  six  actuators/swivels  is  being  presented. 
The  vector  Pb  will  account  for  the  distance  between  the  two  coordinate 
systems .  The  vector  Pb  will  include  the  translational  degrees  of  freedom  of 
the  platform  plus  the  initial  z  offset.  The  initial  z  offset  is  the  nominal 
distance  between  the  platform  and  base  (See  figure  5-3) .  Figure  5-2  also 
introduces  a  new  vector  BSi  which  is  the  vector  from  the  base  coordinate 
system  origin  to  the  platform  swivel  joint.  The  following  vector  algebra 
will  first  determine  the  vector  BSi  and  then  the  actuator  vector  (Li) : 


Pb  = 

X,  Y,  Z 

-Offset 

Offset 

=  125.73 

BSib 

=  Pb  +( 

PRb  *  Ppip) 

b 

(Eq 

6) 

and 

BSib  = 

Bbib  +  Lib 

or  Lib 

=  BSib  -  Bbib 

(Eq 

7) 

Thus 

Lib  = 

Pb  +<pRb  * 

Ppip)  b  “ 

Bbib 

(Eq 

8) 
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Figure  5—4 .  Actuator  Vectors 
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Magnitude  of  Li 


(Eq  9) 


V 


Li,2  +  Li  2  +  Li, 


where  i  =  1,2, 3, 4, 5, 6 

The  actuator  length  is  determined  by  using  the  above  equations.  This  task 
was  accomplished  using  FORTRAN  in  the  subroutine  "ACTUATOR”.  The 
transformation  was  performed  in  subroutine  "FORWARD_TRANS"  and  the  magnitude 
of  the  vector  was  determined  in  subroutine  "NORM".  See  Appendix  D  for 
listings.  Some  of  the  results  produced  by  this  task  are  shown  in  the 
following  table: 


TABLE  5-2  RESULTS  OF  FORWARD  POSITION  KINEMATICS 


Platform 

6  Degrees 

Of  Freedom 

Actuator 

Lengths 

lUx 

lUy 

XU* 

X 

T 

Z 

1 

2 

3 

4 

5 

6 

(Rad) 

(Rad) 

(Rad) 

(In) 

(In) 

(In) 

(In) 

(In) 

(In) 

(In) 

(In) 

(In) 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

160.0 

160.0 

160.0 

160.0 

160.0 

160.0 

0.349 

0.0 

0.0 

0.0 

0.0 

0.0 

136.6 

137.9 

157.9 

162.7 

186.1 

180.9 

0.0 

0.349 

0.0 

0.0 

0.0 

0.0 

146.7 

147.4 

187.1 

187.1 

147.4 

146.7 

0.0 

0.0 

0.349 

0.0 

0.0 

0.0 

141.2 

182.1 

141.2 

182.1 

141.2 

182.1 

0.0 

0.0 

0.0 

30.0 

0.0 

0.0 

149.9 

179.7 

157.3 

157.3 

179.7 

149.9 

0.0 

0.0 

0.0 

0.0 

30.0 

0.0 

149.8 

166.7 

179.4 

144.4 

1S8.7 

175.7 

0.0 

0.0 

0.0 

0.0 

0.0 

30.0 

184.5 

184.5 

184.5 

184.5 

184.5 

184.5 

0.26 

0.26 

0.26 

10.0 

10.0 

10.0 

119.1 

164.9 

178.5 

197.5 

171.3 

185.4 

-0.26 

0.26 

0.26 

10.0 

10.0 

10.0 

151.4 

204.1 

182.6 

194.0 

127.. 

160.4 

-0.26 

-0.26 

-0.26 

10.0 

10.0 

10.0 

203.9 

187.3 

172.5 

124.1 

183.6 

148.4 

0.0 

0.0 

0.0 

10.0 

10.0 

10.0 

160.1 

175.4 

172.4 

161.0 

172.9 

168.9 

The 

results 

of  Task  1 

illustrate 

a  large 

variation  in 

actuator  lengths 

produce  the 

desired 

six 

degrees 

of 

platform 

orientations 

.  The 

first 

case 

considered  the  nominal  position,  where  the  six  degrees  describing  the 
platform  orientation  are  all  zero  which  results  in  actuator  lengths  of  160 
inches.  The  six  degrees  of  freedom  of  the  platform  will  be  considered 
Relative  to  this  nominal  position.  The  results  of  this  task  were  compared 
with  DADS  simulations  for  numerous  different  orientations  of  the  platform, 
and  it  was  found  that  the  same  results  were  produced. 

5.4  NET  PLATFORM  FORCE  AND  TORQUE 

The  next  task  was  to  determine  the  net  torque  and  forces  on  the  platform 
given  the  actuator  force  magnitudes  and  platform  orientatioii .  The  magnitude 
of  the  six  actuators  forces  will  be  denoted  as  Qi.  The  actuator  vector  Li 
will  be  normalized  to  determine  an  actuator  unit  vector  (ULi)  aS  follows: 


ULi  =  Li 

Magnitude  (Li)  (Eq  10) 


where  Magnitude  (Li)  =  i,2  +  Li,2  +  Li,2 

The  actuator  force  vector  (Fai)  will  be  determined  by  multiplying  the 
actuator  force  magnitude  by  the  actuator  unit  vector  as  follows: 

Faib  =  Qi  *  ULib  (Eq  11) 

The  net  external  force  on  the  platform  is  simply  the  sum  of  all  the  actuator 
force  vectors  minus  the  weight  force  in  the  z  direction  as  follows: 


(0,  0,  Mass  *  g)b 


(Eq  12) 


6 

FORCEb  =  Sum  Faib  - 
i=l 

The  torque  applied  to  the  platform  by  each  actuator  is  determined  by  a  vector 
cross  product  as  follows: 

Tai  =  Ppi  x  Fai  (Eq  13) 

In  terms  of  cross  product  operation  on  a  common  coordinate  system  the 
following  equation  is  used: 

Taip  =  Ppip  x  (bRp  *  Faib)p  (Eq  14) 

It  should  be  mentioned  that  these  equations  describe  a  simplified  case.  To 
consider  turret  rotation,  an  additional  torque  applied  by  the  turret  would 
have  to  be  considered.  The  net  torque  on  the  platform  is  simply  the  sum  of 
the  applied  torques  from  each  actuator  as  follows: 

6 

TORQUEp  =  Sum  Taip  (Eq  15) 

i=l 

The  net  force  vector  "FORCE"  is  left  in  base  coordinates  for  the  time  being, 
while  the  net  torque  vector  "TORQUE"  is  left  in  platform  coordinates.  This 
is  due  to  the  convenience  of  determining  the  angular  acceleration  from 
inertia  properties  described  in  platform  coordinates. 

5.5  ACTUATOR  RATES  BY  VECTOR  CROSS  PRODUCT 

The  next  task  was  to  determine  the  actuator  rates  when  the  platform 
orientation  and  rates  are  known.  The  magnitude  of  the  actuator  rate  is 
needed  for  the  comprehensive  model,  because  each  sub-loop  independently 
controls  the  motion  of  each  actuator.  In  other  words,  the  actuator  sub-loop 
simply  sees  the  actuator  motion  in  a  single  axis  translation  sense.  The 
magnitude  of  actuator  rate  is  one  of  the  states  in  the  feedback  sub-loop. 
The  task  will  be  accomplished  by  a  simple  vector  cross  product  and  dot 
product  relationship.  It  will  also  be  done  with  a  different  technique  in 
section  5.6.  The  platform  rates  will  be  described  by  wx,  wy  and  w,  for  the 
general  X,Y  and  Z  axis,  respectively.  In  terms  of  platform  coordinates  the 
angular  rates  wp  will  be  described  by  wpx,  wpy,  and  wp,  accordingly.  The  linear 
(translational)  velocities  of  the  platform  coordinate  origin  will  be 
described  by  V  or  by  Vx,  vy,  and  Vx,  respectively.  The  first  part  of  this 
task  is  to  determine  the  velocity  of  each  of  the  platform  swivel  joints .  The 
position  of  the  platform  swivel  relative  to  the  base  coordinate  origin  is  as 
follows : 

BSi  =  P  +  Ppi  (Eq  16) 

Taking  the  derivative  of  this  vector  will  result  in  the  velocity  of  a  point 
representing  the  swivel  joint  as  follows: 

BSi'  =  P'  +  Ppi'  (Eq  17) 

BSi'  =  V  +  Wp  x  Ppi  (  x  denotes  vector  cross  product  )  (Eq  18) 


In  terms  of  operation  on  a  common  coordinate  system,  the  following  equation 
is  the  final  solution  used  in  the  FORTRAN  program. 


BSV  =  Vb  +  [  pR,  *  (Wp  x  Ppip)p  ]  b 


(Eq  19) 
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Now  that  the  swivel  velocity  is  determined  in  equation  6,  the  magnitude  of 
the  actuator  velocity  rate  can  be  determined  by  the  finding  the  component  of 
swivel  velocity  that  is  in  the  direction  of  the  actuator  vector  (Li) .  This 
is  done  by  a  simple  vector  dot  product  as  follows: 


Magnitude  (LVi)  =  BSib'  .  ULib  (  .  denotes  vector  dot  product  )  (Eq  20) 


Where  ULi  was  discussed  in  the  previous  section. 

5.6  DETERMINING  THE  RATE  MATRIX  (S  MATRIX) 

This  section  is  based  on  technical  notes  from  Contraves  Goertz  Corporation, 
the  contractor  for  the  design  and  development  of  the  TMBS .  The  FORTRAN 
portion  in  Appendix  B  for  this  section  is  simply  labeled  "CONTRAVES  NOTES" 
and  the  corresponding  subroutine  in  Appendix  D  is  called  "SMATRIX" .  The  S 
matrix  can  be  considered  equivalent  to  all  the  operations  described  in  the 
previous  section  combined.  The  S  matrix  was  derived  in  reference  3  &  7.  The 
S  matrix  is  considered  in  this  study  to  be  a  transformation  matrix  to  convert 
the  platform  space  to  actuator  space  in  terms  of  rates.  Utilizing  the  S 
matrix  produces  the  same  numbers  as  those  obtained  by  means  of  the  method 
described  in  the  previous  section.  The  product  of  the  S  matrix  and  platform 
rate  vector  give  the  actuator  rate  vector  as  follows: 

L'  =  S  *  Vp  where  S  is  rate  matrix  (6x6)  (Eq  21) 

Actuator  Rates  L'  =  [  Mag (Li')  i=l, 6  ]T  (6x1)  (Eq  22) 


Platform  Rates  Vp  =  [  w„,wy,wIf V,,,Vy, V,  ]T  (6x1)  (Eq  23) 

Before  deriving  the  S  matrix,  a  matrix  Cr  is  introduced  which  represents  the 
cross  product  operation. 

0  -Wx  Wy  Wx,  Wy,  Wz  are  angular  velocity 

Cr  =  Wz  0  -Wx  components  in  plat from  cordinates 

-Wy  Wx  0 

Property  of  Cr  for  a  vector  V  is  Cr  V  =  (W  x  V) 

The  S  matrix  is  derived  as  follows: 

Lib2  =  (BSib  -  Bbib) 2 

Taking  the  time  derivative  of  both  sides  results  in  the  following: 

2  Lib  Lib'  =  2  (BSib  -  Bbib)T  (BSib'  -  Bbib' ) 
where  Bbi„'  =  0 

and  BSib'  =  (Ppip  +  Pb) '  =  PR„  (Wp  x  Ppip)  +  Pb'  =  (pRb  Cr  Ppip)b  +  Pb' 

Reducing  equation  27  and  substituting  equation  28  results  in  the  following: 

Lib  Lib'  =  [BSib  -  Bbib]T  [  (PR„  Cr  Ppip)b  +  Pb' ] 

=  [Ppip  +  Pb  -  BbiJ1  [(pR*  Cr  Ppip)b  +  Pb']  (Eq  29) 


(Eq  24) 

(Eq  25) 

(Eq  26) 

(Eq  27) 

(Eq  28) 
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Multiplying  equation  29  results  in  the  following: 

Lib  Lib'  =  (Eq  30) 

PpipT  ^  Cr  Ppip  +  PpipT  pRbT  Pb'  +  (Pb-Bbib) 1  pRb  Cr  Ppip  +  (Pb-Bbib)T  Pb' 

Due  to  antisymetric  property  of  Cr  the  relationship  PpipT  Cr  Ppip  =  0  and 
pRp1  pRb  =  I  can  be  used  to  determine  the  following  equation: 

Lib  Libf  =  PpipT  pR,  Pb'  +  (Pb  -  Bbib)r  pR„  Cr  Ppip  +  (Pb  -  Bbib) T  Pb'  (Eq  31) 

=  (Pb  -  Bbib)  pR,  Cr  Ppip  +  (BSib  -  Bbib)T  Pb' 

Solving  for  Lib'  gives  the  following  equation: 

Li„'  =  (1/Li)  [  (BSib  -  Bbib) 1  Pb'  +  (Pb  -  Bbib)T  pRb  Cr  Ppip  ]  (Eq  32) 

Let  Vir  =  (Pb  -  Bbib) T  pRb 

Then 

Lib'  =  (1/Li)  [  (BSib  -  Bbib) T  Pb'  +  Vir  Cr  Ppip  ]  (Eq  33) 

Where  ViT  (Wp  x  Ppip)  =  ViT  Cr  Ppip  =  (Ppip  x  Vi)T  Wp 
The  following  equation  is  the  results: 

Lib'  =  (1/Li)  [  (BSib  -  Bbib) T  Pb'  +  (Ppip  x  Vi)*  Wp  ]  (Eq  34) 

The  S  matrix  can  now  be  derived  by  inspection  of  equation  34.  The  S  matrix 
will  be  partitioned  into  two  submatrices  denoted  as  SA  &  S„  which  will  be 
determined  separately.  The  S  matrix  is  split  up  because  half  of  the  matrix 
describes  rotational  properties  and  the  other  half  considers  translational 
properties.  The  S  matrix  is  partitioned  as  follows,  where  the  rows  for  both 
submatrices  are  indexed  according  to  the  six  actuators.  The  three  columns  of 


both  submatrices  are  indexed  by  the  three  components,  (x, y  and  z) 

S  (6x6)  =  SB  (6x3)  |  SB  (6x3)  (Eq  35) 

The  construction  of  S*  starts  with  the  vector  Vip  calculated  as  follows : 

ViT  =  [  Pb  -  Bbi  ]T  pRt,  or  Vi  =  bRp  [  Pbb  -  Bbib  ]  (Eq  36) 

And  using  equation  30  results  in  the  following  submatrices 

S„  =  (  Ppib  x  Vib  )  /  Magnitude  (Li)  (Eq  37) 

SB  =  (  BSib  -  Bbib  )  /  Magnitude  (Li)  where  i=l, 6  (Eq  38) 

The  S  matrix  is  now  constructed.  The  inverse  of  equation  21  can  also  be  used 
to  solve  for  the  platform  rates  as  follows: 

Vp  =  S'1  *  L'  (Eq  39) 


5.7  DETERMINATION  OF  THE  DECOUPLING  MATRIX  (D  MATRIX) 

The  D  matrix  is  considered  the  dynamic  decoupling  matrix  The  results  of  the 
D  matrix  subroutine  were  compared  with  a  study  conducted  by  Contraves  Goertz 
Corporation  and  are  presented  in  reference  6.  The  results  were  identical  to 
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reference  6,  when  the  configuration  shown  in  Appendix  E  was  used.  The  D 
matrix  is  Contraves  Goertz  Corporation's  solution  to  the  coupling  effects 
present  in  a  multi-actuator  system.  The  D  matrix  will  be  incorporated  in  the 
control  law  of  the  TMBS  to  compensate  for  the  dynamic  coupling  effects  of  the 
independently  driven  actuators.  This  will  be  accomplished  by  software 
operation  which  will  change  the  gains  of  the  sub-loops  "on  the  fly”,  creating 
a  form  of  adaptive  control.  This  technique  will  be  covered  in  more  detail  at 
a  later  time,  when  this  technique  has  been  validated  with  the  comprehensive 
model  and  test  data.  At  this  time,  it  is  considered  another  means  of 
comparing  the  results  of  this  study.  The  D  matrix  is  determined  as  follows 
with  the  introduction  of  an  inertia/mass  matrix  "I". 


I 


0  0 
0  0 
0  0 


0  0 

0  0 

I,  0 

0  m 
0  0 

0  0 


0  0 

0  0 

0  0 

0  0 

m  0 
0  m 


I„,  Iy,  1 1;  Total  Inertias 
m  ;  Total  Mass 


(Eq  40) 


For  this  study  I,,  Iy  =  59000  in-lb-s*  I,  =  118000  in-lb-s2  and  mass 
m  =  468.75  slug 


For  simplicity,  the  origin  of  the  platform  cordinates  will  be  the  center  of 
gravity  (eg)  and  principle  axes  at  this  time.  Further  modifications  in  the 
mass  properties  will  be  evaluated  at  a  later  time. 

In  terms  of  kinetic  energy  (KE)  of  the  platform,  ignoring  the  gyroscopic 
effects,  the  following  equation  is  considered: 

KE  =  1/2  VpT  I  Vp  (Eq  41) 

Substituting  equation  39  gives: 

KE  =  1  L'T  [  S'T  I  S'1  ]  1/  (Eq  42) 

2 


Let  D  =  S'T  I  S"1  (Eq  43) 

KE  =  1  L't  D  L  (Eq  44) 

2 


Considering  equation  44  to  be  analogy  to  a  single  translational  motion  on  a 
mass,  the  matrix  D  can  be  considered  the  effective  mass  on  each  actuator  for 
a  given  platform  orientation  and  mass  properties.  The  D  matrix  will  be 
implemented  into  the  actuator  control  compensation  for  the  rate  loop  and  will 
be  discussed  in  more  detail  when  the  comprehensive  model  is  developed. 

The  inertias  will  be  assumed  to  be  constant  for  this  study:  however,  they 
include  the  total  inertia  of  the  turret  and  platform  and  should  be  considered 
to  be  a  function  of  turret  rotation.  The  comprehensive  model  will  have  to 
account  for  the  change  of  inertia  due  to  the  rotating  turret.  The 
orientation  of  the  actuators  are  also  assumed  to  have  negligible  effects  on 
the  total  inertia  at  this  time. 


5.8  POSITION  INVERSE  KINEMATICS 

The  inverse  of  Task  1  (Section  5.3)  as  follows: 


Given  the  six  individual  actuator  lengths,  what  are  the  corresponding  six 


degrees  of  freedom  of  the  platform  orientation? 

We  use  the  Newton  Raphson  technique  for  solving  a  set  of  nonlinear  equations 
and  was  taken  from  reference  7.  Since  this  is  the  inverse  of  Task  1,  the 
results  were  easily  checked  for  numerous  cases.  The  Newton  Raphson  technique 
uses  an  iterative  process  to  determine  the  solution.  The  software  in  the 
appendices  gives  the  user  the  choice  of  selecting  the  process  to  be  governed 
by  the  number  of  iterations  or  the  acceptable  error  tolerance  (epsilon)  of 
each  degree  of  freedom  of  the  platform.  The  process  initially  requires  a 
guess  on  the  actuator  lengths  as  a  starting  point  which  will  be  denoted  as  a 
6x1  vector  (array)  "Wk”.  The  Wk  array  will  contain  the  updated  actuator 
length  approximation  throughout  the  process  and  should  be  considered  to  be 
more  accurate  after  each  iteration  (for  most  cases) .  The  following  equations 
show  the  basic  technique: 

W  =  (EUx,EUy,EUz,X, Y,Z)T  (Eq  45) 

Let  Fi  =  (  Mag  (Li*)  ]2  -  [  Mag  (Li)  ]2  (Eq  46) 

where  Mag (Li*)  is  the  current  approximation  of  actuator  length  and  Mag (Li) 
which  was  the  actuator  length  given. 

The  desired  solution  is  Fi=0  for  i=l,6  where  Li*  =  Li 

The  Newton  Raphson  method  is  formulated  by  the  following  equation,  where  the 
trial  solution  Wk<01  is  the  initial  guess  followed  by  successive 
approximations  Wk‘n+11 . 


6 

Fi  +  Sum  dFi  (Wk,ntl,-Wkn)  =  0  (Eq  47) 

k=l  dWk 

Let  dFi  =  Tik  where  Tik  will  be  the  elements  of  a  T  matrix  (Eq  48) 

dWk 


Fi  +  T  [  Wk"tl  -  Wkn  ]  =  0  (Eq  49) 

Thus  W"*1  =  W"  -  T'1  *  Fi  (Eq  50) 

The  T  matrix  and  array  Fi  are  calculated  in  the  subroutine  T_MATRIX  in 
Appendix  D.  Equation  50  is  the  main  part  of  the  Newton  Raphson  technique, 
where  the  T  matrix  is  inverted  and  the  main  objective  is  observing  how 
quickly  the  iteration  converges.  The  following  shows  how  the  T  matrix  will 
be  formulated. 

T  Matrix  Formulation 


Equations  7-9,  which  solved  for  the  actuator  vector  will  be  rewritten  (Or 
recall  equation  26)  as  follows: 

Lib2  =  [  BSib  -  Bbib  ]  2  (Eq  51) 

Then  let  Fi  =  [  BSib  -  Bbib  ]2  -  Li2  (Eq  52) 

Taking  the  derivative  with  respect  to  Wk  results  in 

d (Fi)  =  2  *  (BSib  -  Bbib)  *  d(BSib)  (Eq  53) 

d(Wk)  d(Wk) 


Recall  (Eq  6)  where  BSib 


(  pRb  *  Ppip  )  +  Ppib. 


Taking  the  derivative  with  respect  to  Wk  and  considering  Ppib  as  constant 
gives  the  following: 


d(BSi) 

d(Wk) 

=  d(R) 
d(Wk) 

★ 

Ppi 

+  d(P) 
d(Wk) 

(Eq 

54) 

where  the  subscripts 
simplicity. 

'b' 

and  'p'  will  be  eliminated 

for  the  time  being 

for 

d(BSi) 

d(EUx) 

=  d(R) 
d(EUx) 

* 

Ppi 

(Eq 

55) 

d(BSi) 

d(EUy) 

=  d(R) 
d(EUy) 

* 

Ppi 

(Eq 

56) 

d(BSi) 

=  d(R) 

* 

Ppi 

(Eq 

57) 

d(EUz)  d(EUz) 

Recall  (Eq  4)  where  R  =  Rz  *  Ry  *  Rx,  taking  the  derivative  results  in  the 
following: 

d(R)  =  d (Rz)  *  Ry  *  Rx  (Eq  58) 

d(EUz)  d(EUz) 


The  following  rotational  matrix  properties  are  used: 


d(Rx) 

=  Mx 

*  Rx  = 

Rx 

*  Mx 

(Eq 

59) 

d(EUx) 

d  (Ry) 

=  My 

*  Ry  = 

Ry 

*  My 

(Eq 

60) 

d(EUy) 

d  (Rz) 

=  Mz 

*  RZ  = 

Rz 

*  Mz 

(Eq 

61) 

d(EUz) 

Mx,  My,  and  Mz  represent  infinitesimal  rotations  about  the  three  Euler  axis. 
These  matrices  were  calculated  by  taking  the  derivatives  of  the  rotation 
matrices  (Eq  1-3)  ,  with  respect  to  each  axis  of  rotation,  and  applying  zero 
to  represent  small  angular  displacement,  as  shown  in  the  following: 


Mx  =  d(  Rx(EUx)  ) 
d(EUx) 


0  0  0 

0  0-1 

0  1-1 


(Eq  62) 


My  =  d(  Ry(EUy)  ) 
d(EUy) 


0  0  1 

0  0  0 

-10  0 


(Eq  63) 


Mz  =  d(  Rz (EUz)  ) 
d(EUz) 


0-10 
10  0 
0  0  0 


(Eq  64) 


* 


These  matrix  properties  reduce  Equation  59  and  61  to  the  following: 

d(R)  =  Mz  *  Rz  *  Rx  *  Ry  =  Mz  *  R  (Eq  65) 

d (EUz) 

similarly  d(R)  =  Mx  *  R  (Eq  66) 

d(EUx) 
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d(R) 

d(EUy) 

Rz  *  d(Ry)  *  Rx 

d(EUy) 

Rz  *  [  My  *  Ry  ]  *  Rx 

[  Rz  *  My  *  Rz-1  ]  *  Rz  *  Ry  *  Rx 

(Eq  67) 

=  My  *  R  where  My  =  Rz  *  My  *  Rz"1 

From  properties  of  rotation  matrices: 

(Eq  68) 

My  =  My(EUy)  =  -Mx  *  Sin(EUy)  +  My  *  Cos (EUy) 

(Eq  69) 

Now  the 
freedom 

derivative  of  the  BSi  vector  with  respect 
"Wk"  can  be  determined  as  follows: 

to  the  platform  degrees  of 

d(BSi) 

d(Wl) 

=  d(BSi)  =  Mz  *  R  *  Ppi 

d(EUx) 

(Eq  70) 

d(BSi) 

d(W2) 

=  d  (BSi)  *=  My  (EUy)  *  R  *  Ppi 

d(EUy) 

(Eq  71) 

d(BSi) 

d(W3) 

=  d(BSi)  =  R  *  Mx  *  Ppi 

d(EUz) 

(Eq  72) 

d(BSi) 

d(W4) 

=  d(BSi)  =  (1,  0,  0)  =  11 

d(X) 

(Eq  73) 

d(BSi) 

d(W5) 

=  d (BSi)  =  (0,  1,  0)  =  12 
d(Y) 

(Eq  74) 

d(BSi) 

d(W6) 

=  d (BSi)  =  (0,  0,  1)  =  13 
d(Z) 

(Eq  75) 

And  finally  the  T  matrix,  which  can  be  considered  a  constraint  Jacobian 
matrix,  is  formulated  by  all  the  equations  above  in  this  section  as  follows: 


Ll*R*Mx*Ppl 

Ll*My  (EUy) *R*Ppl 

Ll*Mz*R*Ppl 

LI*  11 

L1*I2 

L1*I3 

L2*R*Mx*Pp2 

L2*My  (EUy) *R*Pp2 

L2*Mi*R*Pp2 

L2*I1 

L2»I2 

L2*  13 

L3*R*Mx*Pp3 

L3*My (EUy) *R*Pp3 

L3*Mt*R*Pp3 

L3*I1 

L3*  12 

L3*I3 

L4*R*Mx*Pp4 

L4*My  (EUy) *R*Pp4 

L4*Mz*R*Pp4 

L4*I1 

L4*I2 

L4*I3 

LS*R*Mx*Pp5 

L5*Hy  (EUy) *R*Pp5 

L5*Mz*R*Pp5 

L5*I1 

L5*I2 

LS*I3 

L6*R*Mx*Pp6 

L6*My (Euy)*R*Pp6 

L6*Mz*R*Pp6 

L6*I1 

L6*I2 

L6*I3 

(Eq  76) 


5.9  DYNAMIC  MODEL 

Before  the  dynamic  model  can  be  assembled,  several  other  relationships  must 
be  determined.  The  Euler  equations  for  an  orthogonal  inertia  system  are  as 
follows: 


TORQUE  =  Ip  Wp'  +  (Wp  x  Ip)  Wp 


(Eq  77) 
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Which  results  in: 

TORQxp  =  IXp  *  Wxp'  -  (Iyp  -  Izp)  *  Wyp  *  Wzp  (Eq  78) 

TORQyp  =  Iyp  *  Wyp'  -  (Izp  -  IXp)  *  Wzp  *  WXp 

TORQZp  =  Izp  *  WZp'  -  (IXp  -  Iyp)  *  Wx„  *  Wyp 

where  TORQXp,  TORQyp,  TORQzp  are  net  torques  on  platform 

WXp' ,  Wyp'  and  Wzp'  are  angular  accelerations 
WXp,  Wyp  and  Wzp  are  angular  velocities 
lx,,,  Iyp  and  Izp  are  inertias 


If  the  angular  rates  are  considerably  small,  the  gyroscopic  effects  can  be 
neglected  which  reduces  equation  78  to  the  following: 

TORQXp  =  Ix„  *  Wx/  (Eq  79) 

TORQyp  =  lyp  *  Wyp' 

TORQzp  =  Izp  *  Wzp' 


These  equations  are  applied  in  the  subroutine  "ACCEL"  to  solve  for  the 
angular  accelerations  (Wx' ,  Wy' ,  Wz')  in  platform  coordinates.  Either 
equation  78  or  79  is  used,  depending  on  the  logical  variable  "GYROSCOPIC"  in 
the  subroutine.  Many  trial  runs  have  shown  that  the  gyroscopic  effects  are 
negligible  due  to  the  small  angular  rates  of  the  platform. 

Since  the  nature  of  platform  orientation  is  based  on  Euler  angles  in  this 
analysis,  a  means  must  be  determined  to  transfer  the  platform  (body)  axis  of 
xyz  to  the  Euler  angle  axis  (Axis  in  which  the  rotations  were  actually  made) . 
This  may  not  be  clear  until  one  considers  that  the  Euler  axis  is  not  an 
orthogonal  axis  and  should  not  be  mistaken  with  the  xyz  body  axis.  The 
transfer  is  accomplished  in  terms  of  angular  rates. 

Let  the  cross-product  operation  be  represented  by  a  matrix  (Cr)  as  follows: 

0  -Wzp  Wyp 

Cr  =  Wzp  0  -Wx,,  (Eq  80) 

-Wyp  Wx,,  0 

Where  Wx,  Wy,  Wz  represent  the  components  of  angular  velocity  (W)  in 
platform  cordinates. 

Thus  the  results  give  Cr(3x3,  V(3>1)  =  W  x  V  (Eq  81) 

Where  "x"  represents  vector  cross  product 

The  transformation  matrix  has  the  following  property: 

d  (R)  <=  Cr  R  (Eq  82) 

dt 

Solving  for  Cr  in  equation  82  by  taking  the  derivative  of  the  transformation 
matrix  results  in: 

Wx,,  =  EUx'  -  Sin(EUy)  EUz' 

Wyp  =  Cos (EUx)  EUy'  +  Cos (EUy)  Sin (EUx)  EUz' 


(Eq  83) 


Wzp  =  -  Sin(EUy)  EUy'  +  Cos (EUy)  Cos (EUx)  EUz' 

Solving  for  the  Euler  angle  rates  of  equation  7  0  results  in  the  following: 


EUx'  =  WXp  +  Wyp  *  TAN  (EUy)  *  SIN  (EUx)  +  Wzp  *  TAN  (EUy)  *  COS  (EUx)  (Eq  84) 

EUy'  =  Wyp  *  COS  (EUx)  -  Wzp  *  SIN  (EUx) 

EUz'  =  Wyp  *  SIN  (EUx)  /  COS  (EUy)  +  Wzp  *  COS  (EUx)  /  COS  (EUy) 


The  Euler  angles  (EUx,  EUy,  and  EUz)  are  then  determined  by  integrating  the 
Euler  angle  rates  (EUx',  EUy',  and  EUz'),  which  are  then  applied  to  the  next 
iteration  of  the  simulation.  Shown  in  Figure  5-5  is  the  Dynamic  Model  Flow 
Chart,  which  presents  the  steps  used  for  the  dynamic  model.  The  actuator 
rates  (LDi) ,  D  matrix,  Euler  Parameters  (el,  e2,  e3  and  e4)  and  the  global 
(base  coordinate)  angular  velocities  and  accelerations  (ANGVLb  &  ANGACb)  are 
determined  as  part  of  the  results  used  to  compare  with  other  simulations  but 
are  not  required  in  the  dynamic  simulation  loop  shown.  A  comparison  is  shown 
in  the  following  table  (Table  5-3),  which  compares  the  results  of  this 
analysis  and  a  Dynamic  Analysis  and  Design  System  (DADS)  simulation.  The 
results  shown  are  just  one  of  many  case  runs  trialed.  The  results  shown  in 
Table  5-3  were  produced  by  driving  the  actuators  with  a  16735  pound  force 
except  for  actuator  Q6  which  was  set  at  a  14735  pound  force.  This  was  one  of 
the  trial  runs  chosen  which  exibits  motion  in  all  six  degrees  of  freedom. 
The  time  histories  generated  from  both  models  are  generally  comparable  for 
all  states  of  the  platform  and  actuators. 


DYNAMIC  MODEL  FLOW  CHART 


Ppi,  Bbi,  Offset 

Angular  &  linear  states 

Magnitude  Fai 

Li 

FORCEb,  TORUEp 

ANGACp,  TRACCb 

ANGVLp,  XVLb,  YVLb,  ZVLb 

EUxD,  EUyD,  EUzD 

EUx,  EUy,  EUz,  X,  Y,  Z 

LDi 

D 

eO,  el,  e2,  e3 

ANGVLb,  ANGACb 


*  Not  required  for  dynamic  model 

Figure  5-5. 
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Table  5-3  TMBS  DYNAMIC  MODEL  RESULTS 


Platform  driven  by  six  constant  actuator  forces  as  follows: 

Actuator  Forces  (LBS) 

Q1  =  16735  Q2  =  16735  Q3  =  16735  Q4  =  16735  Q5  =  16735  Q6  =  14735 


ACSL/FORTRAN  MODEL _ DADS  MODEL 

T  =  0  sec.  (Nominal  Position  for  Initial  Conditions) 


Actuator  Lengths 

LI 

160.0030 

160.0033 

(In) 

L2 

160.0030 

160.0033 

L3 

160.0030 

160.0033 

L4 

160.0030 

160.0033 

L5 

160.0030 

160.0033 

L6 

160.0030 

160.0033 

Actuator  Rates 

LD1 

0.0 

0.0 

(In/Sec) 

LD2 

0.0 

0.0 

LD3 

0.0 

0.0 

LD4 

0.0 

0.0 

LD5 

0.0 

0.0 

LD6 

0.0 

0.0 

Euler  Angles 

EUx 

0.0 

_ 

(Rad) 

EUy 

0.0 

- 

EUs 

0.0 

- 

Euler  Angle  Rates 

EUDx 

0.0 

_ 

(Rad/Sec) 

EUDy 

0.0 

- 

EUDz 

0.0 

- 

Translational 

X 

0.0 

0.0 

Position  (In) 

Y 

0.0 

0.0 

Z 

0.0 

0.0 

Translational 

XD 

0.0 

0.0 

Rate  (In/Sec) 

YD 

0.0 

0.0 

ZD 

0.0 

0.0 

Translational 

XDD 

0.0 

0.0 

Acceleration  (In/Sec**2) 

YDD 

0.0 

0.0 

ZDD 

0.0 

0.0 

Angular 

ANGACx 

0.0 

0.0 

Acceleration  (Rad/Sec**2) 

ANGACy 

0.0 

0.0 

ANGACz 

0.0 

0.0 

Angular 

ANGVLx 

0.0 

0.0 

Rate  (Rad/Sec) 

ANGVLy 

0.0 

0.0 

ANGVLz 

0.0 

0.0 

Euler  Parameters 

E0 

0.0 

0.0 

Magnitude 

El 

0.0 

0.0 

12 

0.0 

0.0 

E3 

0.0 

0.0 

T  =  0.1  Sec 

Actuator  Lengths 

LI 

160.7390 

160.7389 

(In) 

L2 

160.2940 

160.2941 

L3 

161.0500 

161.0498 

L4 

160.2880 

160.2878 

L5 

159.3640 

160.3645 

L6 

158.6760 

160.6763 

Actuator  Rates 

LD1 

14.8626 

14.8622 

(In/Sec) 

LD2 

5.8776 

5.8775 

LD3 

21.1902 

21.1899 

LD4 

5.6253 

5.6255 

LD5 

-12.7666 

12.7665 

LD6 

-26.8422 

-26.8418 

Euler  Angles 

EUx 

-0.01098 

_ 

EUy 

0.00772 

- 

EUz 

-0.00526 

- 

Euler  Angle  Rates 

EUDx 

-0.22139 

_ 

(Rad/Sec) 

EUDy 

0.15545 

- 

EUDz 

-0. 10823 

- 

Translational 

X 

0.02200 

0.02199 

Position  (In) 

Y 

-0.02360 

-0.02359 

Z 

0.08198 

0.08200 

Translational 

XD 

0.44319 

0. 44318 

Rate  (In/Sec) 

YD 

-0.47022 

-0.47022 

ZD 

1.64128 

1.64127 

Translational 

XDD 

4.56109 

4 . 56104 

Acceleration  (In/Sec**2) 

YDD 

-4.63357 

-4.63359 

ZDD 

16.48560 

16. 48566 

Angular 

ANGACx 

-2.26071 

-2.26104 

Acceleration  (Rad/Sec**2) 

ANGACy 

1.63192 

1.63114 

ANGACz 

-1.14698 

-1.14731 

Angular 

ANGVLx 

-0.22056 

-0.22056 

Rate  (Rad/Sec) 

ANGVLy 

0.15663 

0.15662 

ANGVLz 

-0.10652 

-0.10652 

Euler  Parameters 

E0 

0.99997 

0.99974 

Magnitude 

El 

0.00548 

0.00548 

E2 

0.00387 

0.000387 

E3 

0.00260 

0.00261 
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Table  5-3  TMBS  Dynamic  Model  Results  (Continued) 


_ ACSL/FORTRAN  MODEL _ DADS  MODEL 

T  =  0.2  sec.  ~~~  "  '  '  " 


Actuator  Lengths 

LI 

163.0370 

163.0361 

(In) 

L2 

161.2040 

161.2031 

L3 

164.3480 

164.3473 

LA 

161.1010 

161.1006 

L5 

157.4580 

157.4586 

L6 

154.5130 

154.5131 

Actuator  Rata* 

LD1 

31.5406 

31.5383 

(In/Sec) 

LD2 

12.4976 

12.4955 

LOS 

45.5623 

45.5617 

LD4 

10.4087 

10.4095 

LD5 

-25.2434 

-25.2426 

LD6 

-57.3397 

57.3381 

Euler  Angles 

EUx 

-0.46495 

(Rad) 

EUy 

0.32289 

_ 

EUx 

0.25333 

- 

Euler  Angle  Rates 

EUDX 

-0.46495 

(Rad/Sec) 

EUDy 

0.32289 

_ 

EUDx 

-0.25334 

- 

Translational 

X 

0.08994 

0.08993 

Position 

(in) 

Y 

-0.09335 , 

-0.09335 

l 

0.32895 

0.32890 

Translational 

XD 

0.92525 

0.92521 

Rate  (Zn/Seo) 

YD 

-0.91946 

-0.91949 

ZD 

3.30214 

3.30211 

Translational 

XDD 

5.14514 

5.14508 

Acceleration 

(In/Sec*»2) 

YDD 

-4.31345 

-4.31353 

ZDD 

16.74060 

16.74063 

Angular 

ANGACx 

-2.48674 

-2.4928 

Acceleration 

(Rad/Sec**2) 

ANGACy 

1.95993 

1.9460 

ANGACX 

-1.54190 

-1.5494 

Angular 

ANCVLx 

-0.45697 

-0.45719 

Rate  (Rad/Sec) 

ANGVLy 

0.33395 

0.33344 

ANGVLX 

-0.23842 

-0.23869 

Euler  Parameters 

E0 

0.999561 

0.999560 

Magnitude 

El 

0.022324 

0.022320 

E2 

0.016000 

0.015998 

E3 

0.011075 

0.011073 

T  =  0.4  sec. 

Actuator  Lengths 

LI 

(In) 

L2 

L3 

L4 

L5 

L6 

Actuator  Rates 

LD1 

(In/Sec) 

LD2 

LD3 

LD4 

LD5 

LD6 

Euler  Angles 

EUx 

(Rad) 

EUy 

EUx 

Euler  Angle  Ratea 

EUDx 

(Rad /Sec) 

EUDy 

EUDZ 

Translational 

X 

Position 

(In) 

Y 

Z 

Translational 

XD 

Rate  (In/Sec) 

YD 

ZD 

Translational 

XDD 

Acceleration 

(ln/Sec**2) 

YDD 

ZDD 

Angular 

ANGACx 

Acceleration 

(Rad/Sec*  *2) 

ANGACy 

ANGACx 

Angular 

ANGVI.x 

Rate  (Rad/Sec) 

ANGVLy 

ANGVLz 

Euler  Parameters 

E0 

Magnitude 

El 

E2 

E3 

173.6080 

173.6065 

165.4010 

165.4003 

180.0050 

180.0027 

163.6400 

163.6405 

150.4310 

150.4314 

135.1140 

135.1168 

78.0155 

78.00747 

30.8977 

30.89224 

117.9770 

117.96610 

12.8095 

12.80859 

-41.2221 

-41.22158 

143.5540 

-143.54290 

-0.198732 

0.134897 

- 

-0.125385 

- 

-1.12557 

_ 

0.72869 

_ 

-0.88484 

- 

0.390504 

0.390460 

-0.356384 

-0.356375 

1.325780 

1.325700 

2.15126 

2.15103 

-1.67064 

-1.67080 

6.65600 

6.65590 

7.2253 

7.22513 

-3.1547 

-3.15488 

16.3519 

16.35225 

-2.91155 

-2.911579 

4.05207 

4.051720 

-3.58219 

-3.581805 

-1.006570 

-1.006509 

0.887446 

0.807393 

-0.715670 

-0.715623 

0.991273 

0.991273 

0.094581 

0.094573 

0.073134 

0.073127 

0.055528 

0.055519 

DADS  -  Dynamic  Analysis  and  Design  System  Double  Precision 

ACSL/FORTRAN  Advanced  Continuous  Simulation  Language  Single  Precision 
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5.10  RESULTS 


The  results  of  this  analysis  have  been  compared  with  DADS  output  for  numerous 
cases.  In  addition,  comparisons  were  made  with  reference  6,  which  was  a  D 
matrix  study  conducted  by  Contraves,  the  contractor  for  the  TMBS.  The 
results  were  in  agreement  when  the  coordinate  configuration  in  Appendix  E  was 
applied.  Comparisons  were  made  in  the  kinematics  and  also  in  each  element  of 
the  D  matrix. 

The  comparisons  with  DADS  illustrate  that  the  FORTRAN/ACSL  model  has  a 
limited  accuracy.  This  conclusion  is  based  on  the  fact  that  DADS  uses  double 
precision  and  is  considered  very  accurate.  The  results  show  that  the 
platform/actuator  positions  and  rates  for  the  FORTRAN/ACSL  model  are  accurate 
only  to  the  third  decimal  place.  The  ACSL/FORTRAN  model  is  limited  to  single 
precision  due  to  the  nature  of  ACSL,  which  only  allows  single-precision 
variables  to  be  passed  over  to  the  FORTRAN  procedurals. 

5.10.1  ASSEMBLING  THE  COMPREHENSIVE  MODEL 

Unfortunately,  the  assembling  of  the  comprehensive  model  has  become  a 
disappointment  at  this  time.  The  first  attempts  to  apply  the  hydraulic 
feedback  control  model  from  reference  2  to  the  dynamic  model  have  resulted  in 
an  unstable  behavior.  Many  trial  runs  were  made.  Some  form  of  numerical 
instability  was  found  in  the  comprehensive  model,  as  a  whole.  One  of  the 
first  cases  ran  was  a  step  command  for  the  actuator  displacement  of  equal 
value  for  each  actuator.  This  would  be  equivalent  to  a  vertical  position 
step  command  in  terms  of  the  six  degrees  of  platform  motion.  At  first,  the 
actuator  responses  were  equivalent:  however,  they  began  to  respond 
differently  after  a  short  time  and  eventually  went  unstable.  Since  each 
actuator  is  precisely  modeled  the  same  way  and  is  driven  with  the  same 
command,  initial  conditions  and  controller  configuration,  it  was  difficult  to 
understand  why  the  actuators  would  respond  differently. 

After  many  trial  runs  with  the  comprehensive  model,  it  was  time  to  go  back  to 
the  kinematic  model  and  check  numbers.  It  was  thought  that  an  undesirable 
torque  is  applied  to  the  platform  causing  angular  drift  motion.  Further 
investigation  was  made  into  the  net  torque  calculations  of  the  platform. 
Since  this  portion  was  determined  in  FORTRAN,  it  was  easy  to  isolate  from 
ACSL  so  that  double  precision  could  be  used.  Many  cases  were  tried  for  the 
nominal  position  by  applying  the  same  magnitude  of  force  for  each  actuator 
(Task  #2  of  main  menu  or  section  5.4).  Theoretically,  for  the  nominal 
position,  having  a  magnitude  of  force  equivalent  for  the  six  actuators, 
should  produce  a  net  torque  of  zero.  In  other  words,  the  torque  produced  by 
each  actuator  should  cancel  each  other  out .  It  was  found  that  as  the 
actuator  force  magnitudes  were  increased  beyond  10,000  pounds  (roughly  the 
value  at  overshoot),  the  net  torque  error  would  become  significant.  But  when 
double  precision  was  applied  to  the  same  FORTRAN  and  case  run,  the  net  torque 
was  zero  beyond  twenty  decimal  places.  From  these  results,  an  attempt  was 
made  to  apply  double  precision  to  the  internal  calculations  among  the  FORTRAN 
procedurals.  However,  this  did  not  solve  the  problem  because  variables 
transferred  between  FORTRAN  and  ACSL  are  restricted  to  single  precision 
format . 

As  a  result  of  the  study,  attempts  are  being  made  to  transfer  the  model  to  a 
simulation  language  which  is  better  suited  for  this  type  of  analysis.  Further 
investigation  is  being  made  using  ADSIM  language.  ADSIM  is  a  simulation 
language  used  specifically  for  the  AD100  (Applied  Dynamics)  computer  system 
which  will  be  used  for  other  TMBS  applications.  So  far,  the  results  show  the 
same  instabilities  which  are  now  believed  to  be  due  to  an  inadequate  control 
scheme  for  the  TMBS . 


Currently  a  more  detailed  model  of  the  TMBS  is  also  being  developed  which 
will  include  such  detail  as  the  swivel  joint  motion  and  turret  motion. 


32 


LIST  OF  REFERENCES 


1.  Stewart,  D.,  "A  Platform  with  Six  Degrees  of  Freedom",  Proc  of  Instn 
Mech  Engrs,  1965-1966,  Vol.  180.  Pt.  1,  No.  15,  pp.  371-386 

2.  Helinski,  Arthur  L.,  Hydraulic  Control  Design  and  Modeling  Techniques", 
RDE  Center  Technical  Report  No.  13419,  U.  S.  Army  Tank -Automotive  Command, 
Warren,  MI  (February  1989) 

3.  Various  unpublished  technical  notes  from  Contraves  Goertz  Corporation, 
Nick  Andrianos 

*  4.  CADS  Inc.,  "DADS  Theoretical  Manual" 

5.  Greenwood,  Donald  T.,  "Principles  of  Dynamics"  1965  7-12  Eulerian 
Angles,  8-2  Equations  of  Motion  in  Terms  of  Eulerian  Angles 

6.  Contraves  Goertz  Corporation,  "The  D-Matrix  Study" 

7.  Contraves  Goertz  Corporation,  "Proposal  for  a  Tank  Motion  Base 
Simulator",  P-256615,  Volume  2-  Appendices  (Equations  for  Dynamics  and 
Kinematic  Study  of  TMBS) 


* 


$ 


33 


34 


c 

* 


APPENDIX  A 

LIST  OF  VARIABLES  AND  NOTATION 


* 

t 


A- 1 


* 


A- 2 


VARIABLES  AND  NOTATION 


Explanation  Equation  Notation 

FORTRAN/ACSL  Variable 

Position  Vectors 

Base  Vectors 

Vector  from  base  coordinate  reference 
to  base  swivel  joint 

Bbi 

BASBAS (i, ICOMP) , 
BASE_BASE ( i , ICOMP ) 

Platform  Vectors 

Vector  from  platform  coordinate  reference 
to  platform  swivel  joint 

Ppi 

PLAPLA (i, ICOMP) , 

P  LAT_P  LAT ( i , ICOMP) 

Base  Swivel  Vector 

Vector  from  base  coordinate  reference 
to  plaforre  swivel  joint 

BSi 

BSWIVb (i, ICOMP) 

Actuator  Vector 

Vector  from  base  swivel  joint  to 
platform  swivel  joint 

Li 

ACTVEC(i, ICOMP) 

Magnitude  of  Actuator  Vector 

Distance  representing  actuator  length 

Mag  Li 

ACTLEN(i) 

Neutral  actuator  length 

Initial  actuator  length  at 
nominal  position 

DNEUT 

Unit  Actuator  Vector 

Unit  Vector  of  Actuator 

ULi  =  Li  /  Mag 

Li  ACTUNI (i, ICOMP) 

Platform  vector  P,  Pb 

Vector  from  base  coordinate  reference 
to  platform  coordinate  reference 

Actuator  and  Swivel  Joint  Velocities 

Pb (ICOMP) 

Platform  Swivel  Velocity 

Point  velocity  representing  platform 
swivel  joint 

d(BSi) 

dt 

PVLxyz 

Actuator  Rate 

Magnitude  of  actuator  rate 

Force  and  Torque  Vectors 

Mag  LDi 

ACTVEL(i) , 

LDi 

LDARRAY (i) 

Actuator  Force 

Actuator  Force  Vector 

Fai 

FOACTb (i, ICOMP) 

Magnitude  of  actuator  force 

Magnitude  of  force  produced  by  actuator 

Q1 

Qi 

Net  platform  force  vector 

Net  force  vector  being  applied  to 
the  platform 

FORCE 

FORCEb( ICOMP) 

Actuator  torque  vector 

Torque  applied  to  platform  per 
actuator 

Tai 

TOACT(i, ICOMP) 

Net  platform  torque  vector 

Vector  for  net  torque  on  platform 

Euler  Anqles 

TORQUE 

TORQUp( ICOMP) 

Euler  Angles 

Euler  angles  representing  angular 
platform  orientation 

EUx,  EUy,  EUz 

EUx,  EUy,  EUz 

Euler  angular  velocities 

EUx',  EUy',  EUz 

'  EUxD,  EUyD,  EUzD 

Time  derivative  of  euler  angles 
representing  Euler  angular  velocities 


Explanation _ Equation  Notation  FORTRAN/ ACSL  Variable 


Platform  Angular  Motion 


Angular  velocity  vector 

Platform  angular  velocity  in 

terms  of  orthogonal  axis  x,y.  a 

W,  (Wx,  Wy,  Wz) 

ANGVLb ( 1 , ICOMP ) 

[Base  coordinates) 
ANGVLp ( 1 , ICOMP) 
[Platform  coordinates] 

Angular  acceleration  vector 

Platform  angular  acceleration 

Transformation  Matrices  (3x3) 

WD,  WDx,  WDy,  WDz 

W',  Wx',Wy',Wz' 

ANGACpd,  ICOMP) 

Rotation  x 

Rx 

Rotation  y 

Ry 

_ 

Rotation  z 

Individual  rotations  about  axis 

Rz 

— 

Forward  transformation 

Forward  transformation  matrix 

(3x3)  from  platform  to  base  coordinates 

Rxyz,  pR*,  R 

TRANS (3, 3) 

Reverse  transformation 

Reverse  transformation  matrix 

(3x3)  from  base  to  platform  coordinates 

Vector  Cross  Product  for  Rate 

Rzyx,  bRp,  R'1 

R_TRANS (3,3) 

(Wp  x  Ppi) p 

Angular  velocity  cross  platform 
vector  In  platform  coordinates 

Velocity  Vectors 

(Wp  x  Ppi)p 

WXRp ( i , ICOMP) 

Platform  velocity  vector 

PVLb,  (XVLb,  YVLb,  ZVLb) 

,  PVLb (ICOMP), 

Velocity  of  platform  coordinate 
reference 

Rate  Matrix  Formulation 

V 

(XVLb, YVLb, ZVLb) 

5  matrix 

(6x6)  matrix  which  transforms 

6  degrees  of  freedom  of  platform 
rates  to  actuator  rates 

S  =  Sa  :  Sb 

Smat (6, 6) 

S  inverse  matrix 

(6x6)  matrix  which  transforms 
actuator  rates  to  6  degrees  of 
freedom  rates  of  platform 

S'1 

Sinmat (6, 6) 

Sub-mat rices  of  S  matrix 

Sa 

COMPA  S(i, ICOMP) 

(6x3)  matrix  which  comprises 
a  portion  of  8  matrix 

Sb 

COMP  B_S ( i , ICOMP) 

Vector  Vi 

Vector  used  to  formulate  S 
submatrix  Sa 

Vector  Pb-Bbi 

ViPr 

VIT (i, ICOMP) 

Vector  from  base  swivel  to 
platform  origin 

Inverse  Kinematic  Process 

Pb-Bbi 

P_MINUS_XI (i, ICOMP) 

Infinitesimal  rotation 

MX 

Mx (3,3) 

matrices 

My 

My(EUy) 

My (3, 3) 

My  PSI (3, 3) 

A -4 


Explanation 


Mz (3, 3) 

FORTRAN / ACSL  Variable 


Mz 

Equation  Notation 


Iterative  Vector 

Wk" 

W  ( i) 

Iterative  vector  describing 

(EUx,  EUy,  EUz, X,  Y, 

Z) 

the  current  six  degrees  of  freedom 
of  platform  orientation 

(for  k=l,2,3,4,5, 

6) 

Next  Iterative  Vector 

Next  Iterative  vector  describing 
the  six  degrees  of  freedom 

Wkn  *  1 

NEXTw(i) 

ID  Matrices 

11 

11 

(3x3)  tero  matrix  except  for  a  1  in 

12 

13 

12 

13 

corresponding  dlagnol  term 

F  vector 

Squared  difference  between  actuator 
length  known  and  determined  from  six 
degrees  of  freedom  platform  approximation 

Fi 

Fi  (i) 

Error  tolerance 

Error  between  six  degrees  of 
freedom  platform  orientation 
(Delta  between  current  and 
previous  iteration) 

Epsilon 

wEPS(i) 

T  matrix  (6x6) 

T  matrix  is  defined  as: 

d (FI)  »  Tlk  where  1  row 

d(Wk)  k  column 

D  matrix  formulation 

T 

Tmat  (6,  6) 

D  matrix 

D  matrix  (6x6)  which  will  be 
used  in  the  controller  to  dynamic 
decouple  the  actuators  in  the  rate 
loops 

D 

Dmat (6,6) 

Inertia/mass  matrix 

Matrix  which  includes  the  net 
inertias  and  mass  of  platform/turret 

Inertia  and  mass 

I 

Amat (6, 6) 

Inertia  array 

Inertia  values  in  platform 
coordinate  reference 

INERTp ( I COMP ) 

Mass 

Net  mass  of  platform/turret 

m 

RMASS 

Inertia/mass  matrix 

6x6  matrix  containing  mass 
and  inertias  in  platform 
reference 

ip 

Amat  (6,  6) 

*  Note:  All  FORTRAN/ACSL  variables  denoted  with  ' i'  refers  to  index  of  actuator 

(1  of  6)  . 

Variables  denoted  with  'ICOMP'  refers  to  index  of  vector  component  where 
IC0MP=1,2  or  3  refer  to  x,y  or  z  respectively. 

Angular  motions  are  used  with  i=l  simply  because  the  common  subroutines  used 
are  written  for  vectors  with  2  dimensional  arrays.  (Angular  motion  is  not 
really  associated  with  an  actuator  index) 

Additional  notation  is  added  to  variable  names  such  as  subscript  'b'  and  'p' 
used  to  denote  base  or  platform  coordinate  reference. 


APPENDIX  B 

LISTING  OF  KINEMATIC  MODEL/FORTRAN  PROGRAM 


B-l 


4k, 


¥ 


B-2 


PROGRAM  TMBS_KI NEMATICS 
A.  L.  HELINSKI 


NOTES:  This  program  Is  simply  an  experimental  study  In  the  kinematics 

of  the  TMBS.  Its  subroutines  will  be  eventually  used  in  ACSL 
or  possible  formatted  for  ADSIM.  The  first  part  concludes  my 
own  study  of  the  dynamics  using  Newton-Euler  equations. 

The  second  part  Is  following  the  Contraves  method  In  determining 
the  S  &  D  matrix. 


All  vectors  related  to  the  actuators  are  formatted  as  follows: 


VECT0R( IACT, ICOMP)  where 

I ACT  Is  ACTUATOR  Index 
ICOMP  Is  COMPONENT  Index 


IACT=1,6 
IC0MP=l,2,or  3 
for  X,Y,or  Z 


This  format  Includes  such  vectors  as  ANGULAR_VEL  because 
of  the  general  used  subroutines. 

The  final  character  of  vector-variables  will  have  a  "b" 

"p".  These  represent  "Base  Cordlnates  (Global)" 

"Platform  Cordlnates  (Body)Local  Cordlnates  "  respectively. 


Cordlnate  systems  and  TMBS  geometry  Is  described  In 
Subroutine  CONFIG. 


REAL  BASBAS(6,3),PLAPLA(6,3) 

REAL  Pb(3),TRANS(3,3),R  TRANS(3,3) 

REAL  F0ACTb(6,3) 

REAL  ACTLEN(6  ,ACTVEC(6,3),T0ACTp(6,3) 
REAL  PLAb(6,3),BSWIVb(6,3) 

REAL  COM  EUz.COM  EUy.COM  EUx 
REAL  EUzTEUy, EUx~ 

REAL  ACT  LEG  C0M(6) 

REAL  COM  x.COM  Y.COM  Z 

REAL  FORCEb  3 )7T0RQUp ( 3 ) , FOACTp (6,3) 

REAL  INERTp{3) , ANGACp(6,3) 

REAL  ACTUNII 6,3) 

REAL  ANGVLb  6.3) , ANGVLp(6,3) 

REAL  WXRp (6,3), WXRb (6,3) 

REAL  PVLb  3),PVLxyz  6,3  ,ACTVEL(6) 


■’VLxyz 


v  up  \  u 


ACTVEL(6) 


ADDITIONAL  VARIABLES  USED  IN  THE  CONTRAVES  SECTION 

REAL  Smat(6,6) ,S1nmat(6,6) ,Dmat(6,6) 

REAL  LD(6),RATE(6) 

ADDITIONAL  VARIABLES  USED  FOR  TRANSFORM  ACTUATOR  LENGTHS  TO 
SIX  DEGREES  OF  FREEDOM 


REAL  L(6),' 
REAL  Mz(3, 
REAL  NEXTw 
DIMENSION 


•),w(6),Tmat(6,6), 
3,3).My(3,3|.Mx  3 
Tw(6),wEPS  6 
N  INDX(6) 


,T1nmat(6,6),Fi (6) 
3,3) ,My_PSI (3,3) 
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CHARACTC0*1  REPLY 

kkkkkkknkkkkkkkr.  rkkkkkkkkknkkkkkkkkkkkkkkkkknnkkkkkkkkkk  s:  ****** /  •* 

************  PARAMETERS  ******************************************** 

kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk 

INERTp(l)  =  59000.  !  RING  Inertia  In  x 

INERTp  2  =  59000.  !  RING  Inertia  In  y 

INERTp  3)  *  118000.  !  RING  Inertia  In  z 

RMASS=468. 75  !  RING  MASS 

0NEUTRAL= 160. 0033  !  ACTUATOR  LENGTH  FOR  NEUTRAL  POSITION 

*********************************************************************** 

*******************--***********************************************^** 

*  Call  CONFIG  to  determine  goemetry  of  platform  and  base. 

*  Creates  the  vectors  to  describe  the  swivel  points. 

* 

CALL  CONFIG (BASBAS.PLAPLA.HGT) 

2000  WRITE(5, 1998) 

1998  FORMAT ( /////////////////////////////////// , 

+  •  This  program  Is  written  strictly  to  play  with', 

+  '  the  kinematics  of  the  TMBS.',/,  Once  every  part', 

+  '  of  this  program  Is  correct  It  will  eventually  lead',/, 

+  '  Into  a  ACSL  simulation  for  a  complete  TMBS  model',//, 

+  •  ********  PLAY  MENU  ****★****',/, 

+  /,'  (1)  Given  the  six  degress  of  freedom',/, 

+  '  What  are  the  actuator  lengths  ?',//, 

+  /,'  (2)  Given  the  six  degress  of  freedom  and', 

♦  '  the  six  actuator  FORCES*,/, 

+  '  What  are  the  net  TORQUE  and  FORCE  on  the  platform?',//, 

+  '  (3)  Given  the  six  degrees  of  freedom  and  the  six', 

+  *  degress  In  rates'./,'  What  are  the  actuator  velocities?', 

+//.'  (4)  CONTRAVES  NOTES  (S  and  D  matrix):',//, 

+  '  (5)  Given  actuator  lengths  what  are  the  six  degrees?’,//, 

+  '  (6)  Stop  Quit') 

WRITE(5, 14) 

14  FORMAT!/  '  ENTER  1,2, 3, 4, 5  or  6  ’ ) 

READ(5,*) IMENU 
IFHMENU  .EQ.  1)G0T0  408 
IFi IMENU  .EQ.  2)G0T0  429 
IF( IMENU  .EQ.  3  GOTO  819 
IF( IMENU  .EQ.  4)G0T0  820 
IFi IMENU  .EQ.  61STOP 
IFi IMENU  .EQ.  5)G0T0  3000 
GOTO  2000 
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************  ************  *********  **************  *******  *******  ************* 
******* ■■  Given  the  6  degrees  of  orientation,  **■  *************** 

*********  what  is  the  length  of  each  actuator.  ******************* 

************************************************************************** 
************************************************************************** 

408  CALL  ASK_0R I ENT ( EUy , EUx , EUz , Y , X , Z ) 

* 

CALL  ACTUAT0R{ Pb , BSWI Vb . ACTVEC , EUx, EUy , EUz , 

+  Y , X , 2 , HGT , BASBAS , PLAPLA ) 

* 

★  ★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★AT***************************-********* 

* 

*  CALCULATE  DESIRED  ACTUATOR  LENGTHS 

* 

* 

*  ACTLEN(IACT)  =  NORM  [  ACTVEC(IACT)  ] 

* 

DO  338  IACT=  1,6 

CALL  N0RM( IACT, ACTVEC, ACTLEN.ACTUNI ) 

338  CONTINUE 

* 


WRITE  OUT  LEG  LEGNTH  COMMANDS 
WRITE (5, 667) 

FORMAT {////,' *******  ACTUATOR  LENGTHS  *****★•) 

DO  341  IACT=1,6 

ACT  LEG_COM( I ACT)=ACTLEN( IACT) -DNEUTRAL 
WRITE(5, 1999) IACT, ACT  LEG  COM  IACT), ACTLEN! IACT) 
FORMAT  ( *  ACTUATOR  LEG  COMMAND  Ml  1  =  ',F12.4, 
4X,  '  LENGTH*  \F12.4) 

CONTINUE 
WRI TE( 5 , 892 ) 

FORMAT!/, '  HIT  RETURN  TO  CONTINUE’) 

READ(5,*j 
GOTO  2000 


* 


* 
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A*****************************:1-***#********************************* 

****  Given  the  actuator  ."CRCES  and  the  6  degree  orientation,  **** 
****  what  Is  the  total  force  and  torque  on  the  platform.  **** 
******************************************** ************************ 


429 

★ 

418 


CALL  ASK_0R I ENT ( EUy , EUx , EUz , Y ,  X ,  Z ) 
WRITE|5,418) 


F0RMAT{//,'  Enter  the  6  actuator  FORCEb  separated', 

+  /, '  by  commas',/,'  for  actuator  1,2, 3, 4, 5, 6' ) 

RE AD ( 5 , * )Q1 , Q2 , Q3 , 04 , Q5 , Q6 

********  ACTUATOR  FORCEb  Q1,Q2,Q3,Q4,Q5,Q6  ************************* 
* 

DETERMINE  FORCE  VECTOR  BY  KNOWING  THE  MAGNITUDE  OF  FORCE 
AND  NORMALIZING  THE  ACTUATOR  VECTOR  (ACTVEC( IACT, ICOMP) 

ACTVEC  IS  THE  VECTOR  DESCRIBING  THE  ACTUTOR  LENGTH  AND 
ORIENTATION. 


448 

* 


411 

410 

★ 

if 

is 

★ 

★ 


CALL  ACTUATOR (Pb.BSWIVb, ACTVEC, EUx, EUy, EUz, Y,X, 
Z,HGT,BASBAS, PLAPLA) 

DO  448  IACT=1,6 

CALL  NORM ( I ACT , ACTVEC , ACTLEN , ACTUN I ) 

CONTINUE 

DO  410  IACT=1,6 
DO  411  IC0MP=1,3 

ACTUNIf IACT, ICOMP)  = 

ACTVEC( IACT, lCOMP)/ACTLEN( IACT) 
CONTINUE 
CONTINUE 


CREATE  FORCE  VECTORS  FOACTb(  IACT,  (  X,Y,  or  Z  ) 

1-6,  (  1,2, 

FORCEb  IN  BASE  CORDINATES 
DO  413  ICOMP=l ,3 


or  3) 


413 

**** 

**** 

**** 

**** 

**** 

* 


431 


FOACTbfl, ICOMP 
FOACTb  2, ICOMP 
FOACTb(3, ICOMP 
F0ACTb(4, ICOMP 
FOACTb (5, ICOMP 
FOACTb (6, ICOMP 
CONTINUE 


=Q1*ACTUNI 

=Q2*ACTUNI 

=Q3*ACTUNI 

=Q4*ACTUNI 

=Q5*ACTUNI 

=Q6*ACTUNI 


1, ICOMP 
2, ICOMP 
3, ICOMP 
4, ICOMP 
5, ICOMP 
6, ICOMP 


CONZ  FORCEb  IN  BASE  CORDINATES  TO  PLATFORM  CORDINATES 
FORCEb: * 


BASE  CORD. 
PLATFORM  CORD. 


FOACTb (IACT,  1,2  or  3) 
FOACTp  (IACT,  1,2  or  3) 


DO  431  IACT=1,6 

CALL  REVERSE_TRANS( IACT, EUy, EUx, EUz, 


CONTINUE 


,  EUy, EUx, EUZ, 
FOACTb, FOACTp) 


★ 

★ 

**** 

**** 

*★★★ 


ACCUMULATE  THE  FORCEb  AND  TORQUp  IN  PLATFORM  CORDINATES 
INITIALS. 

DO  417  ICOMP=l,3 


♦ 


B-6 


417 

* 


* 

* 

* 

* 


446 

* 


* 


414 

* 


444 


* 

* 

* 

* 

★ 


789 

521 

★ 


FOQCEb( I COMP ) =0 .  !  INITIAL  PREVIOUS  A" RAYS  TO  ZERO 

TORQUp( ICOMP)=0. 

CONTINUE 

DO  414  IACT=1,6  !  FORCEb  IN  BASE  CORD. 

FORCEbf l)=F0RCEb( l)+FOACTb( IACT, 1)  !  FORCEb  IN  X 
FORCEb  2  =FORCEb  2  +FOACTb  IACT.2  !  FORCEb  IN  Y 
FORCEb(3)=FORCEb(3)+FOACTb(IACT,3)  !  FORCEb  IN  Z 

DETERMINE  PLATFORM  TORQUE  FROM  EACH  ACTUATOR  IN  PLATFROM  CORD. 

CALL  CROSS ( IACT, PLAPLA, FOACTp,TOACTp) 

WRITE! 5, 446) IACT, TOACTp( IACT, l),TOACTp( IACT, 2), 

+  TOACTp(IACT,3) 

FORMAT!//,  •  TORQUE  FROM  ACT  Ml,'  EUx=  ', 

+  F12.4, ’  EUy=' , F12. 4, '  EUz=  '.F12.4, 

+  /) 

TORQUp ( 1 ) =TORQUp ( 1 )  +  TOACTpf IACT, 1)  !  TORQUp  IN  X 

TORQUp  2  =TORQUp(2)  +  TOACTpf IACT.2)  !  TORQUp  IN  Y 

TORQUp (3)=TORQUp (3)  +  TOACTp( IACT, 3)  !  TORQUp  IN  Z 

CONTINUE 

.  ^ ITE  ( 5  •  !  ' 3  5  • 

FORMAT!//, 

+•  INSTANTANIOUS 
+  ’  X=  '.FIS- 

+'  INSTANTANIOUS 
+  ’  X=  ' , F15. 

+///,’ 

READ(5,*) 


TORQUE  :',/,'  (PLATFORM  CORDS)',/, 

6,'  Y=  * ,F15.6, '  Z=  ' , F15.6//, 

FORCE  :',/,'  (BASE  CORDS)',/, 

6,'  Y=  ' , F15.6, '  Z=  '.F15.6, 

HIT  RETURN  TO  CONTINUE’) 


COMPUTE  LOCAL  PLATFORM  ANGULAR  ACCELERATION 
ANGULAR  ACCELERATION  *  SUM  OF  TORQUp  /  INERTIA 

DO  521  ICOMP=l,3 

ANGACp( 1,1 COMP)  =  TORQUp (I COMP)  /  INERTp( I COMP) 
WRITE{5, 789) ICOMP, ANGACp( 1, 1  COMP) 

FORMAT ( '  ANGACp  ( ' ,  II, ’ )' ,4X,F12.8) 

CONTINUE 

GOTO  2000 
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*  frk  ★  *  it  irk  -kirk  It  it  it  '  ***★★•*★*★★**★*****★★****★*★★★******★**★**★***★★ .'  '  ** 

****  Given  the  6  degree  orientation  and  6  degree  rates,  ******** 
****  What  are  the  actuator  velocities?  ******** 

a******************************************************************** 

*★*★*★*★***★*★******★*★  it********************************************* 

★ 

819  CALL  ASK  0RI£NT(EUy, EUx,EUz,Y,X,Z) 

*  ~ 

WRITE{5,510) 

510  FORMAT ( //, *  Enter  ANGULAR  Velocities  /PLATFORM  Cordlnates  ',, 
+  , *  (In  RAD/sec) ' ,/, '  (x,y,  and  z  Separated', 

+  '  by  commas) ' ) 

READ ( 5 , * ) AGVLpx , AGVLpy , AGVLpz 

DO  3029  IACT=1,6 

ANGVLp( IACT, 1 )=AGVLpx 
ANGVLp  IACT, 2  =AGVLpy 
ANGVLp( IACT,3)=AGVLpz 
3029  CONTINUE 
* 

* 


WRITE(5,516) 

FORMAT ( '  Enter  LINEAR  Velocltles/BASE  CORDS',/, 
'  (X,Y  and  Z  Separated  by  commas)') 

READ{ 5,*) XVLb, YVLb, ZVLb 


* 

PVLbl 

[2 

*  YVLb 

PVLbl 

1 

=  XVLb 

* 

★ 

PVLbl 

3) 

=  ZVLb 

00  367  IACT=1 , 6 

CALL  CROSS (I ACT , ANGVLp .PLAPLA , WXRp )  !  w  x  r  In  plat  cords. 

CALL  FORWARD_TRANS( IACT, EUy, EUx, 

+  EUz,WXRp,WXRb)  !  w  x  r  In  base  cords. 

00  368  IC0MP=1,3 

*  l  point  velocity  on  platform 
PVLxyz ( I ACT , I COMP ) =PVLb ( I COMP ) +WXRb ( I ACT , I COMP ) 

368  CONTINUE 

367  CONTINUE 

* 

* 


* 

* 

* 


* 

* 

* 


499 

* 

5789 

* 

* 

* 


FIND  UL  UNIT  ACTUATOR  VECTOR 

CALL  ACTUATOR ( Pb , BSWI Vb , ACTVEC , EUx, EUy , EUz , Pb , Y, X , 
+  Z,HGT,BASBAS, PLAPLA) 

FIND  UNIT  VECTOR  OF  ACTUATOR 


DO  499  IACT=1,6 

CALL  N0RM( IACT, ACTVEC, ACTLEN, ACTUNI ) 


CONTINUE 


WRITE (5, 5789) 

FORMAT (////, '  ACTUATOR  VECTOR:',///) 


DO  401  IACT=1 , 6 
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* 

* 

*402 

* 

* 

*401 

* 

* 

*404 

* 

★ 

★ 

* 

* 


401 


4446 

4445 

* 

9624 


+ 


WRITE(  5,402)  I  ACT,  ACTLEN(  IACT' ,  ACTUNI ( IAC-" ,  1) , 
ACTUNI (IACT, 2) ,  ACTL.'.I  ( lACT,3) 

FORMAT  ( '  ACT  * ,  I1.3X,  ’  MAGNITUDE*’, 

F9.3.3X,'  ACTUATOR  UNIT  • , •  X=  ’.F6.3.1X, 
'  Y=  '.F6.3.1X, 'Z=  * , F6.3) 

CONTINUE 


WRITE(5,404) 
FORMAT (//, ' 
READ( 5, *) 


Hit  Return  To  Continue') 


+  • 


TAKE  DOT  PRODUCT  OF  ACTUNI  AND  PVLxyz  TO 
FIND  THE  ACTUATOR  VELOCITY 

DO  401  IACT=1, 6 
ACTVEL{ IACT)=0. 

CONTINUE 

DO  4445  IACT=1,6 
DO  4446  IC0MP=1,3 

ACTVELf I ACT ) = 

ACTVEL(IACT)  +  ACTUNI ( IACT, ICOMP)*PVLxyz( IACT, ICOMP) 
CONTINUE 
CONTINUE 

WRITE(5,9624) 

FORMAT (/////////.’  LINEAR  POINT  VELOCITIES’, 

ON  PLATFORM’, //,18X,'  SWIVEL  POINT  VELOCITY  (XYZ)  ', 
ACTUATOR  VELOCITY’,//) 

DO  396  IACT=1,6 


+ 

PVLxyzI IACT 

9623 

FORMAT  (IX,’, 

396 

CONTINUE 

WRITE(5,4507) 

4507 

FORMAT ('  HIT 
READ(5,*) 

GOTO  2000 

l 

n. 

:ti 


.  i  i nv  i 

ICT) 

L.'K 


F12.4.3X.F12.4, 

3X, F12. 4,8X, F12. 4) 


* 


* 
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*«"******************************************************************** 

*******************  FROM  CONTRAVES  NOTZS  ***************************** 
********************************************************************** 

*****  BASED  ON  PROPOSAL  FOR  A  TANK  MOTION  BASE  SIMULATOR  P-25615  *** 
*****  P-25615  VOLUME  2  -  APPENDICES  *** 

********************************************************************** 

********************************************************************** 

*  PART  1  FROM  PAGE  A- 17 

*  SOLVE  FOR  THE  A  AND  B  MATRIX 
********************************************************************** 

* 

ASK  FOR  THE  SIX  DEGREES  OF  FREEDOM 


** 

* 

820 

* 

* 

* 


* 

** 

** 

* 

* 

677 

633 


CALL  ASK_ORIENT(EUy,EUx,EUz,Y,X,Z) 

Find  S  matrix  and  S  INVERSE  matrix 

CALL  SMATRIX(EUy,EUx,EUz, Y,X,Z, 

♦  BASBAS , PLAPLA , HGT, Smat .Sinmat) 


Determine  D  matrix 

CALL  DMATRIX(S1nmat,INERTp,RMASS,Dmat) 


+ 

+ 

+ 

+ 

+ 

+ 

+ 


CONTRAVES  MENU  **  ',////, 


* 

* 

* 

634 

* 

* 

* 


* 

4276 

* 

* 


40 


WRITE  (.5, 633} 

FORMAT!///, '  * 

'  (1)  Given  the  6  actuator  rates,  , 

'  what  are  the  degree  rates?',/, 

'  (Based  from  S  inverse  matrix)',///, 

'  (2)  Given  the  6  degree  rates,  ', 

'  what  are  the  6  actuator  rates?',/, 

'  (Based  from  the  S  matrix)',///, 

'  (3)  Return  to  main  menu') 

READ(5,*)ICH0IC£ 

IF( ICHOICE  .EQ.  3)  GOTO  2000 

I F ( I CHOICE  .EQ.  1)THEN 

WRITE(5,634) 

FORMAT(///,  Enter  the  6  actuator  rates  -  ',/, 
'  LD 1 , LD2 , LD3 , LD4 , LD5 , LD6  separated  by  commas') 

READ(5 ,*)LD( 1) ,LD(2) , LD(3) , LD(4) ,LD(5) , LD(6) 

CALL  RATES( ICHOICE, LD, Smat, Sinmat, RATE) 

DO  4276  IACT=1,6 

ANGVLp( IACT, 1)=RATE{1 ) 

ANGVLp  IACT.2  =RATE  2 
ANGVLp(IACT,3)=RATE(3) 

CONTINUE 


WR ITE  ( 5 , 40 ) RATE  ( 1 ) ,  RATE ( 2^RATE (3^RATE ( 4 ) , 

FORMAT (///////,'  RATES:  s)/!’R  E(6) 

ANGULAR  RATES:',//, 


* 

* 

* 


+  ' 
+  ' 
+  ' 
+  • 
+  ' 
+• 


X  ' ,F9.3, '  RAO/SEC’,/, 

Y  ' , F9.3, '  RAD/SEC’,/, 

2  ' , F9.3, 1  RAD/SEC',///, 
Trans  RATES:'.//, 

X  '  ,F9.3, '  IN/SEC',/, 

Y  * ,F9.3, *  IN/SEC ' ,/, 

Z  ' ,F9.3, '  IN/SEC' ,//, 

Hit  Return  to  Continue') 

READ ( 5 , * ) 


GOTO  677 
ENDIF 


635 


636 


* 

* 


+ 

+ 


IF( ICHOICE  .EQ.  2 ) THEN 
WRITE(5,635) 

FORMAT (///, '  Enter  the  3  angular  rates  (PLATFORM  CORDS)', 
'  x,  y,  z  separated  by  commas',/) 

READ ( 5 , * ) ANGVLp (1,1), ANGVLp (1,2),  ANGVLp (1,3) 

WRITE(5,636) 

FORMAT(///,'  Enter  the  3  Translational  rates,  ' 

'Base  cords',/, 

•  X,  Y,  Z  separated  by  commas',/) 

READ ( 5 , * ) XVLo , YVLb , ZVLb 


RATE( l)=ANGVLp( 1, 1) 

RATE  2  =ANGVLp  1,2) 

RATEi 3  =ANGVLp(l,3) 

RATE  4)=XVLb 
RATE(5  i=YVLb 
RATE  6  =ZVLb 

CALL  RATES( ICHOICE, LD.Smat, SI nmat, RATE) 

GOTO  677 

ENDIF 

************************************************************** 
it  irk  It  irk  it  it -kirk  irk  Irk  Irk  "kit  It  irk  It  it*  It  Irk  it  ******************  *******  it  it  Irk  it  it  it  it 
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*  * 


******************  ********K******.,  .V  *********  ***************** 
********************************************************************** 
******************************************** ************************** 


*  Given  the  Actuator  Lengths,  what  are  the  6  degrees  orientation? 

*  This  section  Is  based  on  notes  from  Contraves  on  the  Inverse 

*  TRANSFORMATION  this  Involves  the  Newton  Raphson  method. 

*  W  =  (EUx,EUy,EUz,X,Y,Z+hgt) 

*  Take  Initial  quess  on  W 


*  Initial  Matrices  based  on  the  generation  of  Infinitesimal  * 

*  rotations  about  x,  y,  and  z.  * 

**★★***★*★**★ ********** ********  ******★*★■*★**★****★★★*****★★★★***** 


3000 


* 


Mx 

(1,1 

=  0. 

Mx 

1.2 

=  0. 

Mx 

1,3 

=  0. 

Mx 

'2,1 

=  0. 

Mx 

2,2 

=  0. 

Mx 

2,3 

=  -1. 

Mx 

3,1 

=  0. 

Mx 

3,2, 

=  1. 

Mx( 

3,3 

=  0. 

* 


My  1,1 
My  1,2 
My(l,3 
My  2,1 
My  2,2 
My{2,3 
My  3,1 
My  3,2 
My { 3 , 3 ) 


=  0. 
=  0. 

=  1. 

=  0. 

=  0. 

=  0. 

=  -1. 
=  0. 

=  0. 


★ 


Mz 

(1,1 

=  0. 

Mz 

'1,2 

=  -1 

Mz 

1,3 

=  0. 

Mz 

2,1, 

=  1. 

Mz( 

2,2 

=  0. 

Mz( 

2,3 

=  0. 

Mz 

3,1 

=  0. 

Mz 

3,2 

=  0. 

Mz 

3.3) 

=  0. 

WRITE(5,3001) 

3001  FORMAt{'  Enter  the  actuator  lengths  separated  by  commas', 
+  '  (In  Inches)', 

+  /  'Example  160. ,  160. ,  160. ,  160. ,  160. ,  160.  * ) 

READ(5,*)L(1),[(2),L(3),L(4),L(5),L(6) 


3067  WRITE(5,3011) 

3011  FORMATC  Iterations  are  governed  by:  ',///, 

+  '  (1)  Number  of  Iterations',//, 

+  '  (2)  An  Epsilon  cost  error  limit',///, 

+  *  Enter  1  or  2' ) 

READ(5,*) IGOV 

IFfIGOV  .NE.  1  .AND.  IGOV  .NE.  2)THEN 
GOTO  3067 
ENDIF 
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A 


•# 


3012 


3013 


if;:gov  .eq.  ijtmen 

WRITE(5,3012) 

F0RMAT('  Enter  limit  on  Iterations') 

READ( 5,*) ITERLM 
ELSE 

WRITE(5,3013) 

F0RMAT(*  Enter  Epsilon  for  each  degree  of  freedom', 
•separated  by  commas',/,'  (EUx,EUy,EUz, ' 


’X,Y,Z) ' ) 

READ(5,*  wEPS(  1)  ,w 
wEPS(5),wEPS  6) 


wEPS(2),wEPS(3),wEPS(4), 


ENDIF 

******************* ********************** ********************** 

*  Take  a  wild  Initial  guess  on  the  six  degrees  * 

*  (In  ACSL  this  will  be  based  on  previous  value)  * 

*  * 

*  w(icomp=l,6)  =  (roll, pitch, yaw, fa, ss, vert)  * 

*************************************************************** 

* 


* 

* 


FIND  AVERAGE  ACTUATOR  LENGTH 


3005 

* 

* 


* 

* 


SUM=0 

DO  3005  IACT=1,6 
SUM=SUM+L( IACT) 
CONTINUE 

AVERAGE=SUM/6. 


-L(6) )/18. )  ROLL  Initial  guess  1 
-L(l))/175.)  PITCH  Initial  guess 
!  YAW  Initial  guess  In  Rads 
!  FA  Initial  guess  In  In 
!  SS  initial  guess  in  In 
!  VERT  initial  guess  In  In 


ITERATIONS 


5000  CALL  TMATRIX( 1 ,w, PLAPLA,BASBAS,HGT,Mx,My,Mz 
+  , Treat, FI) 

* 

*  Find  Tinmat 

★ 


N=6 

CALL  INVERSE_MATRIX( Treat, Tinmat, INDX.N) 

*  Determine  new  guess  on  w(i) 

*  By  Newton  Raphson  method 

*  6  (n+1)  (n) 

*  fl  +  Sum  t df ( 1 )  /  dw(k) ]  *  (w(k)  -  w(k))  =  0. 

*  k=l 

* 

* 

*  or  by  Contraves  notes;  w(n+l)  =  w(n)  -  Tinmat  *  fl 

*  6x1  6x1  6x6  6x1 


DO  3070  IR0W=1 , 6 
NEXTw(IROW)=  0. 

DO  3080  IC0L=1 , 6 

NEXTw( IROW)=NEXTw( IR0W)  - 


Rads 
n  Rads 
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3080 

3070 

★ 


3016 

3019 

* 

*  Write 

* 

3056 

* 

3049 

3040 
3043 

3041 
* 

3045 

it 

3047 

* 

Hr 

3053 

* 

3057 

* 


T i nmat( IROW, ICOL'*F’ <  ICOL) 

CONTINUE  '  ' 

NEXTwf IROW)=NEXTw( IROW)+w( IROW) 

CONTINUE 

IF( I GOV  .EQ.  1)THEN 
IF(ITERATI0N  .LE.  ITERLMJTHEN 
GOTO  3053 
ENDIF 
ELSE 

IEXCEED  =0 
DO  3019  1=1,6 

IF(  FI (I )  .GE.  wEPS(I)  )THEN 
IEXC£ED=1 

WRITE (5, 3016) ITERATION- 1, I 

.fORMAT(2X,I3, ’  Iteration  ',11,*  element  of  w  exceeded’) 
ENDIF 
CONTINUE 
ENDIF 

IF( IEXCEED  .EQ.  1 ) THEN 
GOTO  3053 
ENDIF 

Results  , 

DO  3056  1=1,6 
w(I)=NEXTw(I) 

CONTINUE 

WRITE (5, 3049) ITERATION 
FORMAT ( '  ITERATION  =  ' , 13,///) 

WRITE(5,3043) 

FORMAT!/, '  w  =  roll , pi tch,yaw, f a, ss, vert-hgt  ',/) 

ss®  wjjafiid? 

WRITE(5,3047) 

FORMAT ( '  HIT  RETURN  TO  CONTINUE’) 

READ(5,*) 

GOTO  2000 

ITERATION=ITERATION+l 

DO  3057  1*1,6 
I )=NEXTw{ I ) 

CONTINUE 
GOTO  5000 


END 
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APPENDIX  C 


LISTING  OP  DYNAMIC  MODEL/ACSL  PROGRAM 


■* 


C-2 


PROGRAM  ?VNAMICS 

•**★★*****★*  kkkkk  kick  kkkkk  it  kkkkkkkkkkkkkkkkkkkkk  x„. ..  k  k  k  it  irk  k  irk  Irk  ktt  irk 


«* 

«* 

«** 

"* 

«* 

«* 

»* 

** 

«* 

«* 

*•* 

«* 

«* 
"* 
w  » 


A.  L.  HELINSK1 


All  vectors  related  to  the  actuators  are  formatted  as  follows 

VECTOR(IACT.ICOMP)  where 

IACT  Is  ACTUATOR  Index  IACT=1,6 

ICOMP  Is  COMPONENT  value  IC0MP=l,2,or  3 

for  X,Y,or  Z 

This  format  Includes  such  vectors  as  ANGULAR_VEL  because 
of  the  general  used  subroutines. 

Cordinate  systems  and  TMBS  geometry  Is  described  in 
Subroutine  CONFIG. 


tt 

H 

« 

It 

It 

•  ft 

• 

ft 

ft 

ft 

ft 

ft 

ft 

ft 

H 

It 

It 

n 

it 


clnterval  c1nt=0.05 

"  LEG  COMMAND  FOR  6  ACTUATORS" 

"  REAL  LC0M(6, 1000)" 

UNIVERSAL  GEOMETRY  VECTORS" 

REAL  BASBAS{6,3) , PLAPLA{6,3) ,HGT 
"  VECTORS  DESCRIBING  SWIVELS  AND  ACTUATORS" 

REAL  Pb(3),BSWIVb(6,3),ACTVEC{6,3) 

"  ACTUATOR  MAGNITUDE  AND  UNIT  VECTOR" 

REAL  ACTLENI6) , ACTUNI (6,3) 

"  PLATFORM  FORCEb  AND  TORQUp  " 

REAL  FORCEb (3), TORQUp (3) 

"  ANGULAR  &  TRANS  ACCELERATIONS" 

REAL  ANGACp (6,3) , TRACCb (6 , 3 ) 

"  FAMOUS  ORIENTATION  MATRIX  S  &  S  inverse" 

REAL  Smat(6,6; ,Sinmat(6,6) 

"  VECTORS  USED  FOR  ACTUATOR  RATE  CALCULATION" 

REAL  RATE (6) , LDARRY(6) 

"  FAMOUS  D  MATRIX  Decoupling  Matrix  used  in  contEUxer" 

REAL  Dmat(6,6) 

"  EULER  PARAMETER  e  for  DADS  comparison  " 

REAL  eO  el  e2  e3 

"  GLOBAL  MOTION  Velocity  &  Acceleration  " 

REAL  ANGVLb (6,3), ANGACb ( 6,3) 

"  PLATFORM  RATES  " 

bREAL  ANGVLp(6,3) 

ARRAY  INERTp(3) 

"  INERTIAS  X,  Y,  Z,  PLATFORM" 

CONSTANT  INERTp  =  59000. ,59000. , 118000. 
it  MASS  ** 

CONSTANT  RMASS=192. 

*  NEUTRAL  ACTUATOR  LENGTH" 

CONSTANT  DNEUT=160.0030 

it  it 

<•********************************************************************« 

«********************************************************************* 

h********************************************************************* 

INITIAL 

"hit***** r*************************************************************» 
*********************************************************************  it 

"  CALL  CONFIG  TO  0EFINE  PLATFORM  AND  BASE  VECTORS  " 


03 


PROCEDURAL { FORCEb , TORQUp=Ql , Q2 , Q3 . Q4 , Oc , Qc . . . 

, EUy , EUx , EUz , ACTUNI , r uAPLA , RMASS j 
CALL  NETFORCE ( FORCEb , TORQUp , Q1 , Q2 , Q3 , Q4 , Q5 , Q6 . . . 

, EUy, EUx , EUz , ACTUNI , PLAPLA, RMASS ) 

END$" PROCEDURAL  NETFORCE" 

Hick*  it  irk  irk*  irk -kit -kirk*  irk  "kirk  irk -kit  irk  It  it*  it  it  It  It  Irk  irk  irk  it  it*  irk  irk  It  irk  irk* 
******************************************************* 

"  DETERMINE  ACCELERATIONS" 
******************************************************* 


PROCEDURAL { ANGACp , TRACCb=TORQUp , FORCEb . . . 

, AGVLpx , AGVLpy, AGVLpz , INERTp , RMASS] 

CALL  ACCEL ( ANGACp , TRACCb , TORQUp , FORCEb . . . 

, AGVLpx, AGVLpy .AGVLpz , INERTp , RMASS) 

END$"PROCEDURAL  ACCEL" 

************************************************************** 


"  DETERMINE  ALL  RATES  • 

************************************************************** 
*  * 

AGVLpx= I NTEG! ANGACp! 1, 1) ,0. ) 

AGVLpy=INTEG  ANGACp  1,2  ,0.) 

AGVLpz=INTEG(ANGACp(l,3),0. ) 


ft  ff 


PROCEDURAL! EUxD, EUyD,EUzD=EUx, EUy, EUz. . . 

.AGVLpx, AGVLpy, AGVLpz) 

CALL  EUL£RRATE(EUxD,EUyD,EUzD,EUx.EUy,EUz. . 

.AGVLpx, AGVLpy, AGVLpz) 

END$"0f  euler  rate" 


XVLb=INTEG(TRACCb( 1, 1) ,0. ) 

YVLb=INTEG( TRACCb! 1,2) ,0. ) 

ZVLb=I NTEG {TRACCb! 1,3) ,0. ) 

************************************************************* 
************************************************************** 
EUx=INTEG(EUxD,0. 

EUy=INTEG  EUyD.O. 

EUz=INTEG(EUzD,0. 

Y=INTEG(YVLb,0. ) 

X=INTEG  XVLb,0. 

Z=INTEG(ZVLb,0.  ) 

**************** ********************************************* 


"  DETERMINE  S  AND  S  INVERSE  MATRIX  " 

******** *********************** *****************************9% 


PROCEDURAL(Smat,S1nmat=EUy,EUx,EUz,Y,X,Z. . . 

.BASBAS, PLAPLA, HGT.BSWIVb.ACTVEC) 

CALL  SMATRIX! Sma t, S Inmat, EUy, EUx, EUz, Y,X,Z. . . 

.BASBAS, PLAPLA, HGT.BSWIVb.ACTVEC) 

END$"Of  Smatrlx  procedural" 

******************** ******************************************* 


*************************************************************** 
"  DETERMINE  ACTUATOR  RATES  LD  by  S  MATRIX  method" 

*  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  **  **  *  *  *  *  *  *  *  *  *  *  *  *Tr*  *  *  *  **  *  *  *  *  *  * **  *  **  **  *  **  ***  ft 

PROCEDURAL(LDARRY=Smat, Slnmat, AGVLpx. .. 

, AGVLpy , AGVLpz , XVLb , YVLb, ZVLb , RATE ) 

RATE! l)=AGVLpx 
RATE  2  =AGVLpy 
RATE  3)=AGVLpz 
RATE  4)=XVLb 
RATE  5  =YVLb 
RATE  6  =ZVLb 

CALL  RATES ( 2 , LDARRY , Smat , SI nmat , RATE ) 

ENDS"  OF  RATES" 


04 


n  «t 


-*► 


♦ 


CALL  CONF io (BASBAS, PLAPLA, HGT=) 

ft  ft 

"  INITIAL  CONDITIONS  " 

"  Note  also  In  Integrators" 

"  6  DEGRESS" 

EUx=0. 

EUy=0. 

EUz=0. 

x=o. 

Y=0. 

Z=0. 

"  6  DEGREE  RATES" 

EUxD=0. 

EUyD=0. 

EUzD=0. 

XVLb=0 . 

YVLb=0. 

ZVLb=0. 


if****************************************  ***************************** 


DERIVATIVE 

"  DETERMINE  ACTUATOR  VECTOR  AND  BASE  SWIVEL  VECTOR" 

m  n 


PROCEDURAL ( Pb , BSWI Vb , ACTVEC=EUx . EUy . . . 

, EUz , Y , X , Z , HGT , BASBAS , PLAPLA ) 

CALL  ACTUATOR ( Pb , BSWI Vb , ACTVEC . EUx, EUy . . . 
.EUz, Y,X,Z, HGT, BASBAS, PLAPLA 
ENDS"  OF  PROCEDURAL" 


"  DETERMINE  ACTUATOR  LENGTH  ACTLEN  and  Ln" 

It  ft 

PROCEDURALf  LI, L2 , L3 , L4 , L5 , L6, ACTUNI , ACTLEN=ACTVEC ) 
CALL  NORM  1. ACTVEC, ACTLEN, ACTUNI) 

LI  =  ACTLEN( 1) -DNEUT 

CALL  NORM{2, ACTVEC, ACTLEN, ACTUNI) 

L2  =  ACTLEN(2) -DNEUT 

CALL  NORM?3, ACTVEC , ACTLEN , ACTUNI ) 

L3  =  ACTLEN(3) -DNEUT 

CALL  N0RM(4, ACTVEC, ACTLEN, ACTUNI) 

L4  =  ACTLEN( 4) -DNEUT 

CALL  N0RM(5, ACTVEC, ACTLEN, ACTUNI) 

L5  =  ACTLEN( 5) -DNEUT 

CALL  NORM(6, ACTVEC, ACTLEN, ACTUNI) 

L6  =  ACTLEN(6)-DNEUT 
ENDS"  of  procedural" 


"****************************************************  « 

n********  Magnitude  of  Actuator  Forces  **********  » 
01=16735.55 
02=16735.55 
03=16735.55 
04=16735.55 
05=16735.55 
06=14735.55 

ft  ft 


*****************************************************  ft 


"  DETERMINE  TOTAL  FORCEb  AND  TORQUp  ON  PLATFORM  " 

*****************************************************  t» 
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*****  *r  ■*■***★★*★★*  ***'*★★*★★  *★*★★★**★■****★  ★*★***★*★**★'*★***■*  ★★★*» 
*****r^„  ..  ..**********************★************’********★** *n  .  :** 

LD1=LDARRY( 1 ) 

LD2=LDARRY  2 
LD3=LDARRY  3 
LD4=LDARRY  4 
LD5=LDARRY  5 
LD6=LDARRY  6) 

************************************************************ 

*********************************★***********:*******★***★*** 

"  DETERMINE  D  MATRIX" 

«*  ********************************************************« 

"  PR0CEDURAL(Dmat=S1nmat , INERTp.RMASS) " 

"  CALL  DMATRIXfDmat.Sinmat, INERTp.RMASS)" 

"  END  Of  Dmatrix  procedural" 

******************** *****************************************n 

****************  DETERMINE  EULER  PARAMETERS  *****************" 
it****************  for  comparisons  with  DADS  ****************" 

« **************************************** ******************** 

PROCEDURAL(eO,el,e2,e3=EUx, EUy.EUz) 

CALL  EULERPARAM(eO,el,e2,e3,EUx,EUy,EUz) 

END$"  of  EULER  PARAMETER" 

************************************************************* 
*******  GLOBAL  MOTION  (Inertial  References  Frame)***  " 

*******  Convert  Angular  velocity  and  Angular  Acceleration  " 
*******  -jn  platform  cordl nates  to  base  cordlnates  for  " 

*******  comparison  with  DAOS  output.  * 

***********************************************************  * 

PROCEDURAL ( ANGVLb , ANGACb=EUx , EUy , EUz , AGVLpx , AGVLpy . . . 

.AGVLpz) 

ANGVLp( 1, l)=AGVLpx 
ANGVLp  1,2  =AGVLpy 
ANGVLp ( 1 , 3 j=AGVLpz 

CALL  FORWARDTRANS ( 1 , EUy , EUx , EUz , ANGVLp , ANGVLb ) 

CALL  FORWARDTRANS ( 1 , EUy , EUx , EUz , ANGACp . ANGACb ) 

END$"  Of  Global" 

END$"OF  DERIVATIVE" 

DERIVATIVE 

termt(T  .GE.  5) 

END$"OF  DERIVATIVE" 
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APPENDIX  D 

LISTING  OF  FORTRAN  SUBROUTINES 

COMMON  TO  APPENDIX  B  &  C 
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SUBROUTINE  CONFIG(BA$BAS,PLAPLA,HGT) 

This  subroutine  contains  the  geometry  configuration  of  the  platform 
and  base  of  the  TMBS.  Thus  cne  platform  cordlnate  system  and  base 
cordlnate  system  are  derived  here  by  means  of  determining  the  vectors 
from  the  origin  (center)  to  the  swivel  points. 

INPUT:  NONE 

OUTPUT:  BASBA$( IACT, ICOMP)  IACT=1,6  IC0MP=  1,2  or  3 

(Base  vectors  In  base  cordlnates)  X,Y  or  Z 

PLAPLAf IACT, ICOMP) 

(Platform  vectors  In  platform  cordlnates) 

HGT  (Neutral  position  distance  between  the  base  and 
platform  cordlnate  systems.) 

REAL  BASBAS(6,3).PLAPLA(6,3) 

REAL*8  PI, ANGLE, DELB 


HGT=  153.73  -  28. 


1  INITIAL  DISTANCE  BETWEEN  BASE  &  PLATFORM 


BASE  GROUND  PLATFORM  UNIVERSAL  CORDINATES: 
VECTOR  CONFIGURATION  FOR  CURRENT  DESIGN 


RB=*  RAOIUS  OF  BASE  =  125.0  IN 

"0"  REPRESENTS  ACTUATOR  SWIVEL  POINT 


669 


WRITE(5,669) 
- AtC 


FORMAl 


ACT4 


(’ 
ACT5 


+  ' 

+  ' 

+  ’ 

+  ' 

+  ' 

+  ' 

+  ' 

+  • 

+  ’ 

+  ’ 

V 
+  • 

+  • 

+  ' 

+  • 

+  ' 

+  ’ 

+  ' 

+  * 

+/,’ 

READ(5,*) 


TOP  VIEW  OF  BASE 


120 


I 

'I 


X- 

7“T70 


ACT3  0 


(GUN) 


0  ACT6 
0  ACT1 


0  ACT2 

Hit  Return  To  Continue 


:  /• 
; 

\y: 

•  / 

\Jr 

V) 

'± 

,’V 
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C 

c 

c 

c 


BASBAS!  ACTUATOR  #  ,( 


1  2  or  3) 
X  Y  or 


PI=3. 141592654 

IACT=1 
ANGLE=  0. 
RB=125.0 

DELB  =  4.1288687 


!  INDEX  ACTUATOR 
*  INITIAL  ANGLE 
I  BASE  RADIUS 

I  DELTA  SMALL  ANGLE:SIN-1(  9/125) 


DO  10  1=1,3 

BASBASf IACT, 1)=  RB  *  DC0SD( ANGLE-DELB) 
BASBASi I  ACT , 2 ) =  RB  *  D$IND( ANGLE-DELB) 
BASBAS(IACT,3  =  0. 


!  X  component 
!  Y  component 
!  Z  component 


C 

10 


BASBAS(IACT+1, 1)  =  RB  *  DC0SD( ANGLE- 120. +DELB) 
BASBAS  IACT+1, 2  =  RB  *  DSIND( ANGLE-120. +DELB 

BASBAS  IACT+1, 3)  =  0. 

IACT=IACT+2 

ANGLE=ANGLE-120. 


!  X  component 
!  Y  component 
!  Z  component 


CONTINUE 

★★★★♦★★★★★★a********************************************************** 

WRITE{5,3000)RB 

3000  FORMAT !///////• ***************************** » 

+■/, ****************  BASE  UNIVERSAL  VECTORS***********' 

+,//,'  RADIUS  =  ' ,F6.2, '  IN', 

+  //,35X, 'VECTOR  COMPONENTS',/,'  ACTUATOR’ ,8X, 

+  ' . X . ' ,  8X , ' . Y . ', 

+g^  # _ 2- _ '  ) 

DO  3002  IACT=1,6 

WRITE(5,3010) IACT,BASBAS( IACT, 1) , BASBAS ( I ACT, 2) , 

+  BAS8AS(IACT,3) 

3010  FORMAT (6X, II, 10X,F15.5,8X,F15.5,8X,F10.3) 

3002  CONTINUE 

WRITE(5,3004) 

3004  FORMAT ( '  HIT  RETURN  TO  CONTINUE’) 

READ(5,*) 

itlrkirk 


PLATFORM  UNIVERSAL  CORDINATES 

VECTOR  CONFIGURATION  FOR  CURRENT  DESIGN 


667 


+  ' 
+  ’ 
+  ’ 
+  ' 
+  ' 
+  ' 
•9-’ 
+  ' 
+  ' 


RP  =  RADIUS  OF  PLATFORM  =  100  IN 
"O"  REPRESENTS  ACTUATOR  SWIVEL  POINT 


WRITE! 5, 667) 
FORMAT!' 


ACT4 


ACT5 


Y  / 
I  / 


0  represents  swivel  point’,/, 

TOP  VIEW  OF  PLATFORM  %\f/\ 

. 


./  0 


ACT6 


’’’/' 

’’V 

’’/ 

•’/’ 
»/  » 
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1/  6n 

X. 

i2o~ my 


0  DEG 
(GUN) 


READ(5,*) 


Hit  Return  To  Continue 


PLAPLA(  ACTUATOR  #  ,  (1  2  or  3)  ) 


IACT=  1 
RP=  100. 
ANGLE=~60. 

DELP=  5.1636072 

DO  200  1=1,3 

PLAPLA! IACT, 1] 
PLAPLA(IACT,2) 
PLAPLA? I ACT, 3) 


!  RADIUS  OF  PLATFORM 
!  INITIAL  ANGLE 

I  DELTA  SMALL  ANGLE  BETWEEN  ACTSrSIN  -1  (9/100) 


=  RP  *  DC0SD( ANGLE+DELP) 
»  RP  *  DS I ND ( ANGLE+DELP ] 


PLAPLA  IACT+1,1)  =  RP  *  DCOSDf ANGLE -DELP) 
PLAPLA  IACT+1, 2)  =  RP  *  DSIND(ANGLE-DELP) 
PLAPLA* IACT+1, 3)  =  0. 

IACT  =  I ACT  +  2 
ANGLE  =  ANGLE  -  120. 


I  X  component 
!  Y  component 
I  Z  component 
!  X  component 
!  Y  component 
!  Z  component 

!  INDEX  ACTUATOR 


3014. 

3022 

* 


CONTINUE 
WRITE ( 5 , 301 1 )RP 

FORMAT (///////, ' ***************************** ’ , 

+  /  ****************  PLATFORM  UNIVERSAL  VECTORS*********** 1 
+,//,’  RADIUS  =  ' ,F6.2, '  IN', 

+  //,35X, 'VECTOR  COMPONENTS’,/,’  ACTUATOR’ , 10X, 

+  ’ . -X . ’,10X,’ . Y . ', 

+  10X,’ - Z - ’) 

DO  3022  I ACT=1 , 6 

WRI TE( 5, 3014) I ACT, PLAPLA ( IACT, 1) , PLAPLA( IACT.2) , 

+  plapla(iact  3) 

FORMAT(6X,Il,i2X,F15.5,8X,F15.5,8X,F12.5) 

CONTINUE 


WRITE(5,663) 
FORMAT (’  HIT 


663  FORMAT ( ’  HIT  RETURN  TO" CONTINUE ’ ) 

READ(5,*) 

RETURN 

END 

A***************************************************************************** 

SUBROUTINE  ASK  ORIENT(EUy,EUx,EUz,Y,X,Z) 

* 

*  This  subroutine  asks  for  the  currnt  orientation  of  the  platform 

*  in  terms  of  the  six  degrees  of  freedom. 
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* 

* 

★ 

408 

3025 


REAL  EUx, EUy , EUz 
INPUT:  NONE 

OUTPUT:  EUy,£Ux,EUz,Y,X,Z 


WRITE! 5 ,3025) 

FORMAT (////////,'  Enter  command  EUx,  EUy,  EUz  (In  rads)?', 

+  /,'  X,  Y,  Z  rotations  repectlvely' ) 

READ(5,*)  EUx, EUy, EUz 
WRITE(5,3027) 

3027  FORMAT (////////,'  Enter  F/A,  S/S,  Z  (In  Inches)?', 

+  /,'  X  Y,  Z  respectively') 

READ(5,*)X, Y,Z 

RETURN 
'  END 

***********************  *******  It  it  irk  it  A******************************  irk  it  it  it  it  it  it  Irk  it* 
******************************************************************************** 
******************************************************************************** 
SUBROUT I NE  NORM ( I ACT , VECTOR , MAG , UN I T_VECT0R ) 


* 

* 

* 

* 

* 

* 

* 

REAL  VECTOR{6,3) ,MAG(6) .UNIT  VECT0R(6,3) 

*  FIND  NORM 

MAG (I ACT)  =  SQRTf 
+  VECTOR { I ACT, 1)**2 

+  +  VECTOR  IACT, 2)**2 

+  +  VECTOR  IACT, 3)**2  ) 

*  DETERMINE  THE  UNIT  VECTOR 

UNIT  VECTOR(IACT.l) 

+  “VECTOR! IACT, 1 

UNIT  VECTOR! IACT, 2 
+  “VECTOR  IACT, 2 

UNIT  VECTOR! IACT,3’ 

+  “VECTOR! IACT, 3 


This  subroutine  normalizes  a  vector  Into  a  unit  vector  by 
dividing  each  component  by  Its  magnitude  (norm). 

INPUT:  IACT, VECTOR 
OUTPUT:  MAG,UNIT_VECTOR 


/MAG (IACT) 
/MAG ( IACT) 
/MAG (IACT) 


RETURN 

END 

****************** ************************************************************ 
******************************************************************************** 
********************************************************************************* 


* 

* 

* 

* 

* 

* 

* 

* 

* 

* 


SUBROUTINE  ACTUATOR ( Pb , BSWI Vb , EUx , EUy , EUz , Pb, 

+  Y , X , Z , HGT , BASBAS , PLAPLA ) 

This  subroutine  calculates  the  actuator  vector  describing  the  actuator 
length  and  orientation  In  space  by  Base  Cordlnates. 

INPUT:  EUx, EUy, EUz  (In  rads) 

Y,X,Z  (In  Inches) 

BASBAS, PLAPLA 

OUTPUT:  Pb,BSWIVb,ACTVEC 


REAL  PLAb(6,3) , PLAPLA(6 , 3 ) , Pb (3 ) 

REAL  BASBAS(6,3),ACTVEC(6,3),BSWIVb(6,3) 
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* 

★ 


* 

* 

★ 

12 

it 


511 

13 

512 
* 

it 

*16 

★ 

* 

it 

it 

* 

★ 

ir 

* 

it 

it 

ir 


REAL  Y,X,Z,HGT 

P  VECTOR  IS  REFERENCE  FROM  ORIGIN  OF  BASE  CORDINATES  TO 
ORIGIN  OF  PLATFORM  CORDINATES 
Pb(2)  =  Y  !  Y  component 

Pb(l)  =  X  !  X  component 

Pb(3)  =  HGT+Z  !  Z  component 

DETERMINE  VECTOR  FROM  SWIVEL  PLATFORM  TO  BASE 

WRITE(5, 12) 

FORMAT?//////, '  SWIVEL  POINT  VECTORS 
+  '  (Base  cordinate  origin  to  platfrom  swivel)',///) 

DO  512  IACT=1,6 

CALL  FORWARDTRANS ( I ACT , EUy , EUx , EUz , 

+  PLAPLA, PLAb) 

DO  511  IC0MP=1,3 

BSWIVb( IACT, ICOMP)=PLAb( IACT, I COMP) 

+  +  Pb(ICOMP) 

CONTINUE 

WRITE(5, 13) IACT, BSWIVb( IACT, 1) ,BSWIVb( IACT, 2) , 

+  BSWIVb! IACT.3) 

FORMAT  ( '  ACTUATOR  (Ml,  *)*  ,4X, 'X  ■  '  .F12.5.4X,  *Y* 

+  F9.2.4X, 'Z=  ' ,F9.2) 

CONTINUE 


WRITEI5, 16) 
FORMAT ( // , 
READ(5,*) 


Hit  Return  to  Continue') 


CALCULATE  THE  ACTUATOR  VECTOR  IN  BASE  CORDINATES 

) 


ACTVEC(  ACTUATOR  ,  X  Y  or  Z 
(1-6  1  2  or 


3x1 


=  BSWIVb 
3x1 


BASBAS 

3x1 


DO  336  IACT=1,6 
DO  337  IROW=l,3 
ACTVEC( IACT, I  ROW )  = 


337 

CONTINUE 

336 

it 

CONTINUE 

645 

j. 

WRITE(5,645) 

FORMAt(////// 

T 

669 

DO  890  1=1,6 
WRITE!  5 ,61 
FORMAT ('  t 

690 

CONTINUE 

★ 

★ 

WRITE! 5 ,888) 

*888 

FORMAT( '  HIT 

★ 

★ 

READ(5,*) 

z-,/5 


RETURN 

END 
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*******  ^ ****************************** *************  ********  ****************** 
********  ***********************  A  «.,***  ********  *****  **************  Kr:********* 
************************************************************************** 
************************************************************************** 


SUBROUTINE  FORWARDTRANS(IACT,EUy, EUx, 


REAL  EUy, EUx, EUz 
REAL  TRANS (3, 3) 


,£Uy,fcUx, 

EUz,VECT_INP,VECT_OUT) 


REAL  TRANS{3,3) 
REAL  V£CT_lNP(6, 


3) , VECT_0UT(6,3) 


*  This  subroutine  conZs  vector  in  platform  cordinates  to 

*  vector  in  base  cordinates  by  using  EULER  Angle 

*  TRANSFORMATION. 

* 

*  INPUT:  EUy,  EUx,  EUz  (  ANGLES  IN  RADS) 

*  VECT  INP  (INPUT  VECTOR  IN  PLATFORM  CORDINATES) 

*  IACT"  (INDEX  ACTUATOR) 

*  OUTPUT:  VECT  OUT  (OUTPUT  VECTOR  IN  BASE  CORDINATES) 

*  Trans  (TRANSFORMATION  MATRIX) 

****************************************** ***************ifc^*^r****** 

* 

CALL  Trans_MAT ( EUy , EUx , EUz , Trans ) 

* 

*  VECTOR  FROM  BASE  ORIGIN  TO  PLATFORM  SWIVEL  POINTS 

*  (IN  PLATFORM  CORDINATES) 

* 

*  VECT  INPUT  (IN  BASE  CORDINATES) 

*  “  1-6  1  2  or  3 

* 

*  PLAb  =  Trans  *  PLAPLA 

*  VECT  OUT  Trans  *  VECT  INP 

*  3xT  3x3  3x1 

* 

DO  334  IROW=l,3 

VECT  OUT(IACT. IROW)= 

+  ,  TRANS(IROW,l)*VECT  INP(IACT.l) 

+  +  TRANSt IROW, 2)*VECT~INP( I ACT, 2) 

+  +  TRANS ( IROW, 3)*VECT~INP( IACT.3 ) 

334  CONTINUE 

* 

RETURN 

END 

****************** ***************************************** 
************************************ ************************ 
************ ★★*★*★**★* ************************************* 
SUBROUTINE  REVERSETRANS(IACT,EUy, EUx, EUz, 

+  VECT_INP, VECT_OUT) 


REAL  EUy, EUx, EUz 
REAL  R  TRANS(3,3) 

REAL  VfCT  INP(6,3) ,VECT  OUT(6,3) 


This  subroutine  converts  a  vector  in  base  cordlrtates 
to  a  vector  in  platform  condinates  by  using  EULER  Angle 
TRANSFORMATION. 

INPUT:  EUy,  EUx,  EUz  (ANGLE  IN  RADS) 

VECT_INP  (INPUT  VECTOR  IN  BASE  CORDINATES) 


*  OUTPUT*  VECT  OUT  'OUTPUT  VECTOR  IN  PLATFORM  CORDINATFS) 

R_Trans  {TRANSFORMATION  MATRia; 

* 

*****  INVERSE  TRANFORMATION  MATRIX  (TRANSPOSE  OF  ORIGINAL  Trans) 

* 

CALL  R _Trans_MAT( EUy , EUx, EUz , R_Trans ) 


*  PLATFORMJ/ECT  =  R_Trans  *  BASE_VECT 

★ 

DO  343  IROW=l,3 
VECT  OUT ( IACT, IROW)= 

+  R  TRANS( IROW, 1)*VECT  INP(IACT,1) 

+  +  R"TRANS  IROW,2)*VECT~INP  IACT, 2} 

+  +  R_TRAN$( IR0W,3)*VECT~INP( IACT.3) 

343  CONTINUE 
* 

RETURN 

END 

*********************************************************************** 

*********************************************************************** 

★a********************************************************************* 

SUBROUTINE  CR0S$(1ACT,VECT_1,VECT_2, PRODUCT) 

it 

*  This  subroutine  determines' the  cross  product  of  two  vectors. 

* 

*  PRODUCT  =  VECT  1  .CROSS.  VECT  2 


INPUT:  VECT  1,VECT  2 
OUTPUT:  PRODUCT 


( IACT, ICOMP) 


REAL  VECT_1 ( 6 , 3 ) , VECT_2 (6,3), PRODUCT (6,3) 


VECT  1  (IACT  ,  ( 

"  (  1-6  , 

PRODUCT! IACT, 1)  = 


(  X, Y,  or  Z 
1,2,  or  3 


PRODUCT! IACT, 1)  = 

VECT  1( IACT, 2)*VECT  2(IACT,3) 

-  VECT~2(IACT,2)*VECT~1( IACT.3) 

PRODUCT ( IACT, 2)  =  -1.* 

(  VECT_1( IACT, 1*VECT_2(  IACT.3] 


(  VECT  1( IACT, 1  )*VECT  2C IACT.3) 

-  VECT~2( IACT, 1)*VECT~1( I ACT, 3) ) 

PRODUCT! IACT, 3)  = 

VECT  1  (IACT, 1)*VECT  2(IACT,2) 

-  VECT-1( IACT,2)*VECT~2( IACT, 1) 


!  X  PRODUCT 


!  Y  PRODUCT 


!  Z  PRODUCT 


RETURN 

END 

C 

**★★***★★*****★*★***★**★*★★★*★*★★★*****★***★★**★*★****★★★★***★***★★* 

*****★*★*★***★************★*******"******★*****★**★★***★******★*★**** 

★★★★★★★★★★★★★★★★★★★★★★★★★★★★I************************************************ 

Hr*************************************************************************** 

SUBROUTINE  Trans  MAT(EUy, EUx, EUz, Trans) 

* 

*  This  subroutine  calculates  the  forward  TRANSFORMATION  matrix 

*  derived  from  Euler  Angles. 
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*  * 


INPUT:  EUy, cuZ, EUx 
OUTPUT:  TRANS(3,3) 

REAL  EUy, EUx, EUz 
REAL  TRANS (3, 3)  ! 


(In  RADS) 


(3,3)  !  TRANS (ROW, COLUMN) 


TRANS ( 1,1)  =  COS(EUz)*COS(EUy) 
TRANS ( 1,2)  =  -SIN(EUz)*CO$(EUx 


TRANS ( 1,3)  =  3IN(£Uz)*SIN(EUx)  ' 

+SlN( EUy)*COS( EUz )*COS(EUx) 
TRANS(2, 1)  =  COS(EUy)*SIN(EUz) 

TRANS(2,2)  =  COS(EUz)*COS(EUx) 

+SIN( EUz )*$IN( EUy)*SIN(EUx) 
TRANS(2,3)  =  -COS  EUz  *$IN  EUx 


+SIN(EUy)* 
j I N ( EUz ) *S 1 
+SlN( EUy)* 


*COS(EUz)*SIN(EUx) 


TRANS ( 

;3,i) 

=  -SIN 

(EUy) 

TRANS 

3.2 

=  COS( 

EUy)*SIN| 

(EUx) 

TRANS ( 
RETURfi 

3,3) 

1 

=  COS( 

EUy )*COS( 

!eux) 

Ititititirit****************************************:************************** 
**★★★★★*★*★*  ********^*iV*****7it**-*^-Ac±^:**Tt****^*T4rilr********************yr*Ty'** 

SUBROUTINE  R_TR ANS_MAT ( EUy , EUx, EUz, R  Trans) 

* 

*  This  subroutine  calculates  the  reverse  TRANSFORMATION  matrix 

*  derived  from  Euler  Angles. 

* 

*  INPUT:  EUy, EUx, EUz  (In  rads) 


INPUT:  EUy, EUx, EUz  (In  rads) 

OUTPUT:  R_TRANS(3,3) 

REAL  EUy, EUx, EUz 

REAL  R_TRANS(3,3)  !  R_TRANS( ROW, COLUMN) 

R  TRANS (1,1)=  COS(EUz  )*COS(EUy) 
RlTRANS(2,l)=  -SIN(EUz)*COS(EUx) 

+SIN( EUy )*COS(EUz)*bIN(EUx) 

R  TRANS(3, 1)=  SIN( EUz )*SIN(EUx) 

+SIN(EUy)*COS(EUz)*COS(EUx) 

R  TRANS( 1,2)=  CO$(EUy)*SIN(EUz) 

R  TRANS(2,2)=  COS( EUz )*C0S( EUx) 

+SIN(EUzj*$IN(EUy)*SIN(EUx) 
R  TRANS(3,2)=  -COS ( EUz )*SIN( EUx) 

+SIN(EUz)*SIN(EUy)*COS(EUx) 
R  TRANS ( 1,3)=  -SIN(EUy) 

R~TRANS(2,3)=  COS(EUy)*SIN{EUx) 
R~TRANS(3,3)=  COS(EUy)*COS(EUx) 


IN ( EUy) 

;(EUy)*SIN(EUx) 

;(EUy)*COS(EUx) 


RETURN 

END 

*★  ★  ★  it  *  *  ★  it  *  +c  *  fr  *  -k  -k  ★  *  it  tfr  -k  k  *  k  k  *  k  A  ★  *  ★  ★  ★  *  *  ★  it  k  k k  k  k  kk  kk  *  ★★  *****  ★★  fc  k  k  ★  ★★  k  k  it  k  ★  k  k  *  *  ★  *  k  k 
kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk 
kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk 
kkkkk  ★***★*  ********  *  ******************************************************* 

SUBROUTINE  SMATRIX(EUy,EUx,EUz,Y,X,Z, 

+  BASBAS,PLAPLA,HGT,Smat,S1nmat) 


INPUT: 


EUy, EUx, EUz, Y,X, Z.BASBAS, PLAPLA 


D-  10 


OUTPUT :  Smat.Sinmet 

This  subroutine  solves  for  the  S  matrix  and  S  Inverse  matrix 
described  In  the  Contraves  notes. 

ilt  @T®f 

s  ia»r-* 

rvtui'lir  T  kinv  t 


DIMENSION  INDX( 6) 


Pb(  1)  =  X 
Pb  2  =  V 

Pb  3  =  Z+HGT 


SOLVE  FOR  V  VECTOR 


DO  3456  IACT=1,6  - 
DO  3457  IC0MP=1,3  ^ 

CONTINUE 

CONTINUE 

Determine  vJ=  (  P-X1)T0  R  from  contraves  notes 


CALL  Trans_MAT(EUy,EUx, EUz, Trans)  !  Find  Trans  (R  matrix) 


VIT 

(1x3) 


P  MINUS  XI 
(1x3)  ~ 


VIT  [x  y  z  1=  [  x  y  z  1  o 


o  Trans 

(3x3) 

•  Rll  R12  R13  j 

R21  R22  R23 

R31  R32  R33  j 


DO  111  IACT=1,6 

VIT ( IACT , 1)= 

P  MINUS  XI 
+  P “MINUS  XI 
+  P“MINUS_XI 

VIT(IACT,2)= 

P  MINUS  XI 
+  P“ MINUS~XI 
+  P“MINUS_XI 

VIT( IACT ,3)  = 

P  MINUS  XI 
+  P“MINUS_XI 
+  P"MINUS_XI 


IACT, 1)*TRANS  1,1 
IACT, 2  *TRANS  2,1 
I ACT , 3 ) *TRANS (3,1) 

1 1  ACT , 1 ) *TR ANS (1,2) 
IACT, 2  *TRANS  2,2 
[  IACT,3)*TRANS(3,2) 

[  IACT, 1)*TRANS( 1,3] 
IACT, 2)*TRANS  2,3 
IACT,3)*TRANS(3,3 j 


!  X  COMPONENT 


!  Y  COMPONENT 


!  2  COMPONENT 


D-ll 


Ill 

* 

* 

* 

* 

* 

* 

it 

* 

* 
it 


CONTINUE 

Determine  the  A  component  of  Matrix  S 
A  =  (Us  x  Vi )k/l 1  1=1,6  k=l,3 

II  actuator  length 
Us  PLAPLA  platform  vectors 
VI  Determined  above 


( C0MPA_S ) 


GET  ACTUATOR  VECTOR  AND  BSWIVb 

CALL  ACTUATOR! Pb , BSWIVb , EUx, EUy , EUz , Pb , 

+  Y,X,Z,HGT,BASBAS, PLAPLA,) 

DO  112  I ACT= 1,6 

CALL  NORM ( IACT.ACTVEC, ACTLEN.ACTUNI) 

CALL  CROSS! I  ACT, PLAPLA, VI T, COMP A  S) 

DO  113  IC0MP=1,3 

COMPA  S( IACT, ICOMP)  = 

+  "COMPA  S{ IACT, ICOMP)/ACTLEN( IACT) 

CONTINUE 
CONTINUE 


113 
112 
* 

irk*  it  it  irk  it  irk  it  It 
*  ‘ 


DETERMINE  B  PORTION  OF  S  MATRIX  ************** 


1889 

1888 

★ 

★ 

* 

k 

it 

* 


DO  1888  IACT=1,6 
DO  1889  ICOMP=l,3 

COMPB  S!  IACT, ICOMP)  = 

+  (  BSWIVb( IACT, ICOMP)  -  BASBAS( IACT, ICOMP)  )/ 

+  ACTLEN(IACf) 

CONTINUE 
CONTINUE 


19 

18 

13 


17 

15 

21 

* 

* 

* 

* 

* 


JOIN  THE  S  MATRIX  FROM  A  &  B 

6x6  6x3  J  6x3 

S  MATRIX  =  [  COMPA_S  j  COMPB_S  ) 

DO  18  IACT=1,6  ' 

DO  19  ICOMP=l , 3 

Smat( IACT, ICOMP)  =  COMPA  5( IACT, ICOMP) 
Smet( IACT, ICOMP+3)  =  COMPB  S( IACT, ICOMP) 
CONTINUE 
CONTINUE 
WRITE! 5 , 13 ) 

FORM At (////////////,’ 

//) 

DO  15  IROW=l, 6 

WRITE (5, 17)Smat( IROW, 1) ,Smat(IROW,2) , 

-+  Smat ( IROW, 3) ,Smat ( IROW, 4) ,5mat( IROW, 5) ,sn 


**  S  MATRIX  ** 


niu  i  t  \  Ji  it  j  \  i.  r wn  •  j,  i  •  wma  b  i  i  ru/n  *  t  i  « 

!  IROW, 3) , Smat! IROW, 4) , Smat (I ROW, 5) , Smat! IROW, 6) 
FORMAT ( IX, F9. 4,3X, F9. 4,3X, F9. 4,3X, F9.4.3X, F9.4, 


+  3X.F9.4,/) 

CONTINUE 
WRITE(5, 21) 
FORMAT (//, ' 
READ(5,*) 


Hit  Return  to  Continue*) 

Find  the  S  Inverse  matrix 

Set  dummy  matrix  (DUM_MAT)  before  calling  Inverse 


D- 12 


* 


* 


♦ 


★ 


1235 

1234 


00  1234  1=1,6 
DO  1235  J=l,6 
DUM  MAT(I, J)=Smat(I,J) 
CONTINUE 
CONTINUE 


N=6 

* 

CALL  INVERSE  MATRIX(DUM  MAT.Sinmat, INDX.N) 

* 

WRIT£(5, 10) 

10  FORMAT(//, ********  S  INVERSE  MATRIX  (Jocobi an)', 

+  i  *******  //) 

DO  20  IR0W=l!6 

WRITE(5, 40)S1nmat( IROW, 1) , S1nmat( IR0W,2) , 

+  S inmat ( I  ROW, 3) , Sinmati IROW, 4) , 

+  Sinmati IROW, 5) ,Sinmat(IR0W,6) 

40  FORMAT (1X,6(F9.4,3X),/) 

20  CONTINUE 

WRITE(5 , 669) 

669  FORMAT (//, '  HIT  RETURN  TO  CONTINUE') 

READ(5,*) 

RETURN 
.  END  • 


*************************************************************************** 


* 

* 

it 

* 

★ 

* 

★ 

* 


SUBROUTINE  S  MATRIXES  MAT ,S  INV  MAT,EUy,EUx,EUz,S  S.F  A, VERT, 
BASE_BASE,PLAT_PLAT7HGT,BffSE_5WIV,ACT_VECT)  -  “ 

INPUT:  EUy , EUx, EUz , S  S.F  A, VERT, BASE  BASE, PLAT  PLAT 

OUTPUT:  S_MAT,$_INV_MAT  ~  “ 

This  subroutine  solves  for  the  S  matrix  and  S  Inverse  matrix 
described  in  the  Contraves  notes. 


REAL  P  VECT{3) , TRANS (3, 3) , PLAT  PLAT(6,3) 

REAL  BASE  BASE(6,3) .BASE  PLAT (5, 3) .BASE  SWIV(6,3) 
REAL  P  MINUS  XI (6,3) , COMPA  S(6,3) ,C0MPB~S(6,3) 
REAL  VlT(6,3j,S  MAT(6,6)  “ 

REAL  ACT  UNIT! 673). ACT  VECTf 6,3) ,ACT  LENGTH(6) 
REAL  S  INV  MAT (6, 6) ,  DUM  MAT  6,6 
DIMENSION  I NDX ( 6 ) 


* 

* 

* 

■  * 
* 
* 


P  VECT(l)  =  F  A  IX 
P“VECT  2  =  S~S  -  !  Y 
P“VECT(3)  =  VFRT+HGT  !  Z 


SOLVE  FOR  V  VECTOR 


DO  3456  I ACT = 1 , 6 
DO  3457  IC0MP=1,3 

P  MINUS  XI(IACT.ICOMP)  = 


D- 13 


P  VECTOC0MP)  -BASE  BASE( IACT . ICOMP) 

CONTINUE  “ 

CONTINUE 

T  T 

Determine  VI  =  (  P-X1)  o  R  from  contraves  notes 

CALL  TRANS_MAT ( EUy , EUx, EUz , TRANS )  !  Find  TRANS  (R  matrix) 


* 

VIT 

P  MINUS  XI 

0 

TRANS 

* 

★ 

(1x3) 

(1x3)  “ 

(3x3) 

Hr 

!  Rll 

R12 

R13 

★ 

VIT  [k  y  z  ]= 

[  x  y  z  }  o 

R21 

R22 

R23 

Hr 

R31 

R32 

R33 

3457 

3456 

* 

★ 

*** 

* 


* 


* 


* 


* 


111 


* 

* 

* 

★ 

* 

* 

* 

* 

* 

* 


113 

112 


DO  111  IACT=1,6 


VIT ( IACT, 1)=  !  X  COMPONENT 

+  P  MINUS  XI(IACT,1)*TRANS(1,1) 

+  +  P— MINUS  XI  IACT, 2  *TRANS  2,1) 

+  +  P— MINUS- XI IACT,3)*TRAN$(3,1) 


+ 

+ 

+ 


VIT ( IACT.2)= 

P  MINUS  XI ( IACT, 1)*TRANS 
+  P"MZNUS“XI  IACT,2)*TRANS 
+  P"MINUS“XI(IACT,3)*TRANS 


Y  COMPONENT 


VIT ( IACT, 3 )=  !  Z  COMPONENT 

+  P  MINUS  XI ( IACT, 1)*TRANS( 1,3) 

+  +  P— MINUS  XI  IACT, 2  *TRANS  2,3 

+  +  P~MINUS~XI ( I  ACT, 3)*TRANS(3,3) 

CONTINUE 


Determine  the  A  component  of  Matrix  S  (COMPA  S) 
A  =  (Us  x  VI ) k/1 1  1=1,6  k=l , 3 

11  actuator  length 
Us  PLAT_PLAT  platform  vectors 
Vi  Determined  above 


GET  ACTUATOR  VECTOR  AND  BASE_SWIV 
DO  112  IACT=1,6 

CALL  NORM! IACT, ACT  VECT.ACT  LENGTH, ACT  UNIT) 
CALL  CROSS! IACT, PLfiT  PLAT, VlT, COMPA  S)~ 

DO  113  IC0MP=1,3  “  - 

COMPA  S( IACT, ICOMP)  = 

+  “COMPA  S( IACT, IC0MP)/ACT  LENGTH(IACT) 

CONTINUE 

CONTINUE 


* 

★ 


DETERMINE  B  PORTION  OF  S  MATRIX  ************** 


DO  1888  IACT=1,6 
DO  1889  IC0MP=1 , 3 

COMPB  S( IACT, ICOMP)  = 

+  (  BASE  SWIV(IACT, ICOMP)  -  BASE  BASE (IACT, ICOMP)  )/ 

+  ~  ACT  LEflGTH(IACT) 


m 
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CONTINUE 

CONTINUE 

JOIN  THE  S  MATRIX  FROM  A  &  B 


6x6 

S  MATRIX  = 


6x3 

[  COMPA  S 


6x3 

COMPB  S  ] 


DO  18  IACT=1,6 
DO  19  I COMP-1, 3 

S  MAT ( IACT, ICOMP )  =  COMPA  S( IACT, ICOMP) 
S— MAT ( IACT, ICOMP+3 )  =  COMPB  S( IACT, ICOMP) 
CONTINUE 
CONTINUE 

Find  the  S  inverse  matrix 


Set  dummy  matrix  (DUM_MAT)  before  calling  Inverse 


DO  1234  1=1,6 
DO  1235  J=l, 6 
DUM  MAT(I, J)=S  MAT ( I , J ) 
CONTINUE 
CONTINUE 


CALL  INVERSE  MATRIX (DUM  MAT.S  INV  MAT, INDX.N) 

*  —  ~ 

RETURN 

END 

★★★♦★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★■a****************************** 
************************** **************************************************** ****** 
SUBROUTINE  INVERSE_MATRIX(A,AINV, INDX.N) 

******  This  subroutine  Is  the  general  Inverse  matrix  subroutine  It  also 
******  USes  subroutines  LUBKSB  &  LUDCMP.  These  routines  are  set  to  solve 
******  a  6X6  matrix  at  this  time. 


INPUT: 


Matrix  to  InZ 
Work  space  Array 
Order  of  Matrix 


OUTPUT:  AINV  Inverse  Matrix  of  A 

REAL  A(6,6),AINV(6,6) 

DIMENSION  INDX (6) 


lNDX( 6) 


*  Set  initial  AINV  matrix  to  a  identity  matrix 

DO  100  1=1, N 
DO  100  J=1,N 

* 

AINV(I, J)=0.0 

IF( I  .EQ.  0)  AINV( I , J)=l. 0 


CONTINUE 

CALL  LUDCMP ( A, N,N, INDX.D) 
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* 

it 

200 

it 

it 


DO  200  1=1, N 

CALL  LUBKSB(A,N,N, INDX, AINV( 1, I ) ) 
CONTINUE 


RETURN 

END 

************** * * ** ***************** *************************************** 
**************  **  *******  ******* ***********************************  ********* 
SUBROUTINE  LUBKSB(A,N,NP, INDX.B) 

REAL  A ( 6 , 6 ) ,B(6) 

DIMENSION  INDX(6) 

11=0 

DO  12  1  =  1, N 
LL=INDX(  I ) 

SUM=B(LL) 

B(LL)=B(  I ) 

IF  t II.NE.0ITHEN 
DO  11  J=I 1,1-1 

SUM=SUM-A( I, J)*B( J) 

11  CONTINUE 

ELSE  IF  (SUM.NE.O. )  THEN 
II=I 
ENDIF 
B(  I )=SUM 

12  CONTINUE 

DO  14  I=N . 1,-1 
SUM=B(I) 

IF( I . LT.N)THEN 
DO  13  J=I+1,N 

SUM=SUM-A( I , J)*B( J) 

13  CONTINUE 
ENDIF 

B( I )=SUM/A( 1,1) 

14  CONTINUE 
RETURN 
END 

************************************************************************* 
******************************** ***************************************** 
**  This  routine  does  a  upper  and  lower  decomposition  on  the  matrix  ** 


SUBROUTINE  LUDCMP(A,N,NP, INDX.D) 

PARAMETER  (NMAX=100, TI NY=1 . 0E-20 ) 

DIMENSION  A(6,6), INDX(6) , VV(NMAX) 

D=l. 

DO  12  1=1, N 
AAMAX=0. 

DO  11  J=1 , N 

IF  (ABS(A(I, J) ) .GT.AAMAX)  AAMAX=ABS(A( I , J)) 

11  CONTINUE 

IF  (AAMAX.EQ.O. )  PAUSE  'Singular  matrix.' 

VV( I )=1 ./AAMAX 

12  CONTINUE 

DO  19  J=1,N 

IF  (J.GT.l)  THEN 
DO  14  1=1, J-l 
SUM=A( I , J) 

IF  ( I .GT. 1 ) THEN 
00  13  K=  1 , 1  - 1 
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* 


* 


% 


13 


14 


15 


16 


17 


18 

19 


SUM=CIJM-A(  I ,K)*A(K, J' 
CONTINUE 
A(I, J)=SUM 
END  I F 
CONTINUE 
ENDIF 
AAMAX=0. 

DO  16  I=J,N 
SUM=A( I ,  J) 

IF  (J.GT.l)THEN 
DO  15  K=l, J- 1 

SUM=SUM-A( I ,K)*A(K, J) 
CONTINUE 
A( I , J)=SUM 
ENDIF 

DUM=VV( I )*ABS(SUM) 

IF  (DUM.GE. AAMAX)  THEN 
IMAX=I 
AAMAX=DUM 
ENDIF 
CONTINUE 

IF  (J.NE.IMAX)THEN 
DO  17  K=1,N 
DUM=A(IMAX,K) 

A( IMAX,K)=A( J,K) 
a|j,K)=DUM 
CONTINUE 

INDX( J)=IMAX 
IF(J.NE.N)THEN 

IF(AfJ,J).EQ.O.)A(J,J)=TINY 
DUM=1./A( J, J) 

DO  18  I=J+1,N 
A(I,J)=A(I,J)*DUM 
CONTINUE 
ENDIF 
CONTINUE 

IF(A(N,N).EQ.0. )A(N,N)=TINY 
‘"TURN 


RE 

END 


★  ★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★TAT 


************  enq  OF  MATRIX  INVERSION  ROUTINES  ****************************** 
****************************************************************************** 
* 

SUBROUTINE  DMATRIX(S1nmat, INERTp.RMASS.Dmat) 

*>  • 


*  This  subroutine  determines  the  D  matrix  from  contraves  notes  by 

*  the  following; 

* 


* 

* 

Dmat  = 

TRANSPOSE ( SI nmat )  *  Amat  * 

Slnmat) 

* 

INPUT; 

Slnmat, INERTp.RMASS 

* 

* 

OUTPUT; 

Dmat 

* 

* 

Where 

JX  0  0  0 

0  0 

* 

0  JY  0  0 

0  0 

* 

Amat  =  0  0  JZ  0 

0  0  JX, JY, JZ  INERTIAS  RESPECTIVELY 

D- 17 


M  MASS 


Determine  the  A  matrix  AND  TRANSPOSE(A_INV_MAT) 

REAL  Amat(6,6) , I NERTp ( 3 ) , RMASS,Dmat(6,6) 

REAL  S Inmat (6, 6) , S1nmat_TRANS(6,6) , AS1nmat(6,6) 


!  Set  A-  mat  dlagnol  to  It's  values. 


DO  10  1=1,6 
DO  20  J=l,6 

Amat( I , J)=0. 0  !  INITIAL  A-  mat  as  zero 

Slnmat  TRANS ( J, I )=S1 nmat( I ,  J)  !  TRANSPOSE  the  Slnmat 
20  CONTINUE 

10  CONTINUE 

Amat(l,l  =INERTp(l)  !  Set  A-  mat  dlagnol  to  It's  values. 

Amat  2,2  =INERTp  2 
Amat (3, 3  =IN£RTp(3) 

Amat(4,4'i=RMASS 
Amat(5,5  =RMASS 
Amat(6,6)=RMASS 

irk  irk  irk  irk  ir*  *****  ********  ★**★**★  **rt**yr*/r*rt  ic-kirkiridrkit 

*  !  ASInmat  =  Amat  *  Slnmat 

CALL  MATRIX_MULT_6X6(Amat, Slnmat, ASInmat) 

I  Dmat  =  Slnmat  Trans  *  ASInmat 
CALL  MATRIX_MULT_6X6(S1nmat_Trans, ASInmat, DmatT 


Amat  *  Slnmat 


WRITE(5, 1723) 

FORMAT!///////////, '  *******  D-MATRIX  *******',///) 

DO  1724  IR0W=1 , 6 

WRITE( 5, 1726)Dmat ( IROW, 1) ,Dmat ( I  ROW, 2) ,Dmat ( IROW,3) , 
Dmat  IROW.4  .Dmat  IR0W,5),Dmat(IR0W,6) 
FORMAT (1X,6(F9.4,3X),/) 

CONTINUE 

WRITE(5,700) 

FORMAT!//,'  Hit  Return  to  Continue') 

READ(5,*) 


Hit  Return  to  Continue') 


RETURN 

END 

Irk'klck'kick'kfclekidt'kirk'k’k'tcfek'k'fc'k'kirklck'kirfcIck'kiriekHicick'kirkicA 

***  First  Find  ASInmat  =  Amat  *  Slnmat 

*★★**★★*★★***★*★★***★ ****★★*•**★***★*★★★*★★*★**★***★**★★* ***************** 
irk'k'kitirkitfrkieitir'k'fr'Prk'kirii  fr  ★**•*#★* 

★★********★**#★★*★***  rt  ***•*■**★£•***★★•*  ***★*★★★****★*★*★★★**★★***★★*★★★*★*★* 

SUBROUTINE  MATRIX_MULT_6X6( A, B, PROD) 

*  This  subroutine  multiplies  two  matrices  (6x6)  together  by 

*  .  the  following: 

*  PROD  =  A  *  B 


INPUT:  MATRIX  A  and  MATRIX  B  (6x6) 

OUTPUT:  PRODUCT  (6x6) 

REAL  A(6,6),B(6,6) , PROD (6, 6) 


DO  10  1=1,6 
DO  10  J=l, 6 

PR0D(I, J)=0.0  !  Initialize  summation  to  0. 

DO  10  K=l, 6 

PROD( I , J)  =  PR0D( I , J)  +  A(I,K)*B(K,J)  !  COLUMN  ROW  MULTIPLICATION 


D- 18 


RETURN 

ENO 

******************************************************************************* 
********************************************************************************** 
SUBROUTINE  RATES ( I CHOI CE,LD,Smat, Si nmat, RATE) 

* 

*  This  subroutine  computes  the  6  degree  rates  from  the  6  actuator  rates 

*  or  computes  the  6  actuator  rates  from  the  6  degree  rates. 


or  computes  the  6  actuator  rates  from  the  6  degree  rates. 

Input:  ICHOICE, L0{ 6) , SI nmat (6, 6} ,Smat(6,6) 

Output:  RATE ( 6 )  or  L0(6) 


ICHOICE, LD( 6) , S1nmat(6, 6) ,Smat(6,6) 
RATE (6)  or  LD(6) 


ICH0ICE=1 

OR 

ICHQICE=2 


RATE(6)  or  LD 
-1 

LD  =  S  o 
6x1  6x6 


RATE  =  S  o 
6x1  6x6 


where;  LD  =  6  ACTUATOR  RATES 
RATES  =  [3  ROTATIONAL 


3  TransLATIONAL] 


REAL  LD(6) , Smat(6,6) , SI nmat (6, 6), RATE! 6) 
************************************************************************ 

IF( ICHOICE  .EQ.  1)THEN 
DO  10  IR0W=1,6 

RATE! IROW)=Q. 

DO  20  ICOL=l, 6 

RATE( IROW)=RATE(IROW)  +  S1nmat( IROW, IC0L)*LD( ICOL) 

20  CONTINUE 

10  CONTINUE 

* 

ENDIF 

★ 

******************  ^  ************************************************************** 
* 

IF( ICHOICE  .EQ.  2)THEN 

* 

DO  110  IROW=l , 6 
LD( IROW)=0. 

DO  120  ICOL=l , 6 


120 

CONTINUE 

110 

* 

CONTINUE 

WRITE(5, 140) 

140 

FORMAT (///////// 

• 

DO  777  1=1,6 

WRITE! 5,141)1  ,i 

141 

FORMAT! ’  L  ( 

777 

* 

CONTINUE 

WRITE(5, 778) 

778 

FORMAT!//, *  Hit 

* 

READ(5,*) 

★ 

ENDIF 

, '  ACTUATOR  RATES;',///) 
LD(  I ) 

',11,')  =  ’.F9.2) 


Hit  Return  To  Continue*) 


RETURN 

END 
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*************************** *************************-'+******** 
★ 

****************** *************** *************************** 


★ 

★ 

* 

* 

* 

* 

* 

* 

it 

it 

it 

it 

* 

★ 

* 

★ 

* 

* 

* 

* 

* 

★ 

* 

* 

* 

it 

it 

it 

it 

it 

it 

it 

it 

it 

it 

it 

it 

it 

it 

it 

it 

it 

it 

it 

it 

* 

* 

* 

it 

it 

it 

it 

★ 

* 


SUBROUTINE  TMATR I X ( L , W , P LAT_P LAT , BASE_BASE , HGT , Mx , My , Mz 
+  ,Tmat,Fi) 

This  subroutine  determines  the  T  matrix  &  FI  array  which  are 
based  from  Contraves  notes  which  Is  used  for  Inverse  Kinematics 

INPUT;  L(6)  Actuator  Lengths 

W(6)  6  degree  appoxlmatlons 

PLAT  PLAT  { IACT, ICOMP ) 

BASE~BASE  (IACT, ICOMP 
HGT  ~ 

Mx(3,3)  Based  on  Infinitesimal  rotations 


INTERNAL; 

TRANS(3,3) 

My PS I (3, 3) 

M  U( IACT, ICOMP)  1  M  *  PLAT  PLAT 

R“M  U( IACT, ICOMP)  !  R(Trans)"*  M  *  PLAT  PLAT 


OUTPUT  Tmat(6,6) , FI (6) 

T  MATRIX  CONSTUCTION  BASED  ON  CONTRAVES  NOTES 


Ll*Rbp*Mx*Ul 

Ll*My_PSI*Rbp*Ul 

Ll*Mz*Rbp*Ul 

L1*P1 

L1*P2 

L1*P3 

L2*Rbp*Mx*U2 

L2*My_PS I *Rbp*U2 

L2*Mz*Rbp*U2 

L2*P1 

L2*P2 

L2*P3 

L3*Rbp*Mx*U3 

T=  2* 

L4*Rbp*Mx*U4 

L3*My_PS I *Rbp*U3 

L3*Mz*Rbp*U3 

L3*P1 

L3*P2 

L3*P3 

L4*My_PSI*Rbp*U4 

L4*Mz*Rbp*U4 

L4*P1 

L4*P2 

L4*P3 

L5*Rbp*Mx*U5 

L5*My_PSI*Rbp*U5 

L5*Mz*Rbp*U5 

L5*P1 

L5*P2 

L5*P3 

L6*Rbp*Mx*U6 

L6*My_PSI*Rbp*U6 

L6*Mz*Rbp*U6 

L6*P1 

L6*P2 

L6*P3 

where: 


LI  Actuator  Vector 

Rbp  Forward  TRANSFORMATION  matrix 

Mx,My  PSI.Mz  Infinitesimal  rotation  matrices 

U1  PTetform  Swivel  Vector 

P1,P2,P3  Basis  Vectors  x,y,z 


"ACT  VECT(1. ICOMP)" 
"Trans  (3x3)" 

"Mx , My  PSI.Mz" 
"PLAT_PLAT( IACT, ICOMP) 
"Not  used  directly" 


REAL  L(6),W(6),HGT 

REAL  PLAT_PLAT(6,3) ,BASE_BASE(6,3) 


t 
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:(6,6)  .'H6),HCT  VECT(6.3)  .BASE  ( 
1,3  ,My(3,3  ,Mz(3,3),My  PSIi-,37 
.,PITCH,YAW,$S,FA,VERT,TRANS(3,3 


* 

* 

* 


* 

* 


PEAL  Tmat(6. 

REAL  Mx(3,‘ 

REAL  ROLL, 

REAL  LGUESS(6),UGUE$S(6,3) 


SWIV(6,3) 

) 


Dummy  Arrays  for  multiplying 

REAL  M  U(6,3),R  M  U(6,3),L  R  M  U(6) 
REAL  RlU(6,3),M_R_U(6,3),L_M_R_U(6) 


ROLL 

PITCH 

YAW 

FA 

SS 

VERT 


* 

•k 

* 


* 

* 

* 


338 

** 

* 


★ 

* 

* 


Find  actuator  vector  for  this  current  guess 

VERT=VERT -HGT  !  AVOID  CONSIDERING  HGT  TWICE 

CALL  ACTUATOR! Pb,BSWIVb,ACTVEC. ROLL, PITCH, YAW, SS, FA, VERT, 
+  HGT, BASE_BASE, PLAT_PLAT) 

LGUESS(IACT)  =  NORM  [  ACT_VECT(IACT)  ] 

DO  338  IACT=  1,6 

CALL  NORM( IACT, ACT  VECT.LGUESS.UGUESS) 

CONTINUE 

Get  TRANSFORMATION  matrix  for  future  use 

CALL  Trans_MAT( PITCH, ROLL, YAW, Trans)  !  GET  R  (Trans) 
Determine  Infinitesimal  rotation  matrix 


•k 

* 

* 


10 

* 

* 

* 


(YAW 

YAW 

1,1 

=  -Mx 

1,1 

>*SY  +  My 

11,1) 

1,2 

=  -Mx 

1,2 

)*SY  +  My 

1,2 

1,3 

=  -Mx 

1,3 

*SY  +  My 

1,3) 

2,1 

=  -Mx 

2,1 

*SY  +  My 

2,  i  j 

2,2 

=  -Mx 

2,2 

*SY  +  My 

2,2 

2,3 

=  -Mx 

2,3 

*SY  +  My 

2,3 

3,1 

=  -Mx( 

3,1 

*SY  +  My 

3,1 

3,2 

=  -Mx 

3,2 

*SY  +  My< 

3,2 

(3,3) 

=  -Mx( 

3,3 

*SY  +  My( 

3,3) 

Construct  Column  1  of  T  matrix 
DO  10  IACT=1, 6 

CALL  MULT3X3  3X1( IACT.Mx.PLAT  PLAT.M  U) 

CALL  MULT3X3~3X1  IACT, Trans, M"U,R  M  U) 

CALL  MULT1X3~3X1  IACT, ACT  VECT.R  H  U,L  R  M  U) 
Tmat( IACT, 1)=2.*L  R  M  U(lACT)  “  ~ 

CONTINUE 

Construct  Column  2  of  T  matrix 
DO  20  IACT=1, 6 
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CALL  MULT3X3  3X1  ( I AfT ,  Trans ,  PLAT  PLAT ,  R  ij) 
CALL  .-iJLT3X3— 3X 1  f  I  ACT,  My  PSI.R  U7M  A  U)~ 

CALL  MULT  1X3  3X 1 ( IACT.ACT  VECT7M  R"U7L  MRU) 
Tmat( IACT,2)=2.*L  M  R  U(lSCT)  ~  " 

20  CONTINUE 

* 

*  Construct  Column  3  of  T  matrix 

* 

DO  30  IACT=1, 6 

*  R  U  Is  Predetermined  above 
CALL  MUUT3X3  3Xl(IACT,Mz,R  U,M  R  U) 

CALL  MULT1X3“3X1  IACT.ACT  VECT7M“R  U.L  MRU) 
Tmat( IACT,3)=2.*L  M  R  U(lECT) 

30  CONTINUE 

* 

*  Construct  Column  4,5  6  of  T  matrix 

★ 


40 

* 

* 

* 

* 

* 

*441 

* 

* 

* 

*445 

*444 

★ 

*446 

* 

* 


50 


DO  40  IACT=1 , 6 

Tmatf I ACT, 4) =2.* (ACT  VECT(IACT,1 
Tmat  IACT.5  =2.*  ACT“VECT  IACT.2 
Tmat( IACT,6)=2.*(ACT~VECT ( I ACT, 3 
CONTINUE 

Construct  FI  array  which  Is  actuator  length  [Gue$s**2-  Actual*2] 
used  as  cost  function  for  the  Inverse  kinematic  estimation. 


***  T-MATRIX  **  ’,//) 


WRITE(5,441) 

FORMAT (////////////,* 

DO  444  1=1,6 

WRITE(5,445)Tmat(I,l),Tmat(I,2),Tmat(I,3), 
Tmat ( I ,4) ,  Tmat (1,5) ,Tmat ( I ,6) 

FORMAT (6(2X,G10.3)) 

CONTINUE 

WRITE(5,446) 

FORMAT ( '  HIT  RETURN  ') 

READ(5,*) 

DO  50  IACT=1 , 6 

Fi ( IACT)=LGUE$$( IACT)**2-L( IACT)**2 
CONTINUE 


RETURN 

END 

•kirk  ★***★***★******★★*★★*  *******  ****★★*★*  irkiticiekittckit'k'kirk'kfrltirk'trkirk 
'kk’kiricirk'kirkir'A-kickirkkirkickk’kirkkickidtiridrkirkirirkfrk'kkicie'k'kidrkifirkieick’kkitirkir 


SUBROUTINE  MULT3X3_3X1 ( I ACT.MAT3X3 .MAT3X1 , MATRIX?) 

★ 

*  This  subroutine  multiplies  a  matrix  by  a  vector  as  follows; 

•*  MATRIXP  (3x1)  =  MAT3X3  (3x3)  *  MAT3X1  (3X1) 

*  where  the  vector  Is  dimensioned  also  by  IACT  (Index  of  actuator  #) 

*  for  convenience. 

* 

*  INPUT;  MAT3X1( IACT, I COMP)  I  VECTOR 

*  IACT  !  INDEX  representing  actuator  # 

*  MAT3X3( ROW, COLUMN) 

* 

*  OUTPUT;  MATRIXP( IACT, ICOMP)  !  PRODUCT  MATRIX 

* 

* 


REAL  MAT3X1(6,3) ,MAT3X3(3,3) 
REAL  MATRIXP(6,3) 
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*  Initialize  product  to  zero 

* 

DO  95  1=1,3 

MATRIXP( IACT, I )=0. 
95  CONTINUE 

* 

00  100  1=1,3 


MATRIXP( IACT , I )  =  MATRIXP ( IACT, I )  +  MAT3X3(I,1)  *  MAT3X1! IACT, 1)  + 
+  MAT3X3( I ,2)  *  MAT3X1(IACT,2)  +  MAT3X3(I,3)  *  MAT3X1( IACT, 3) 


CONTINUE 


RETURN 

END 

*★★******★*★■*★*  *********************************************************  ****** 
************ **★*★★***★***★*★★* ********************* ************************** 
SUBROUTINE  MULT1X3_3X1( IACT, MAT1X3.MAT3X1, SCALER) 


Is  subroutine  multiplies  a  (TRANSPOSE! vector]  by  a  vector 
SCALER  =  MAT1X3  (1x3)  *  MAT3X1  (3X1) 


as  follows; 


*  where  the  vector  Is  dimensioned  also  by  IACT  (Index  of  actuator  f) 

*  for  convenience. 


INPUT;  HAT 1X3 ( IACT, I COMP)  !  VECTOR 


IACT 

MAT3X1(IACT) 
OUTPUT;  SCALER(IACT) 


!  INDEX  representing  actuator  I 
!  VECTOR 

!  SCALER 


REAL  MAT 1X3 (6, 3) ,MAT3X1(6,3) 

REAl  SCALER(6) 

*  Initialize  product  to  zero 

* 

SCALER( I ACT ) =0 . 

* 

SCALER(IACT)  =  SCALER(IACT)  +  MAT1X3( IACT, 1)  *  MAT3X1( IACT, 1]  + 
+MAT1X3( IACT, 2)  *  MAT3X1< IACT, 2)  +  MAT1X3( IACT, 3 )  *  MAT3X1( IACT, 3) 

RETURN 

END 

***************************************************** ********************** 
************** ******* ****************************************************** 
SUBROUTINE  ACCEL (ANGACp,TRACCb,T0RQUp,F0RCEb, 

+  AGVLpx, AGVLpy , AGVLpz, INERTp, RMASS) 

*  This  subroutine  determines  the  acceleration  of  the  platform  given 

*  the  forces  and  torques  on  the  platform. 

*  INPUT:  AGVLpx, AGVLpy, AGVLpz 

*  TORQUp, FORCED 

*  INERTp, RMASS 

*  OUTPUT:  ANGACjd  (Angular  Acc.  local -platform  cords.) 


REAL  ANGACp(6,3) 
REAL  TRACCb(6,3) 


(Translational  Acc.  base  cords) 


!  Only  (1,3)  Is  used 
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****** 


it 

It 

* 

* 

★ 


* 


it 


REAL  T0RQUo(3),F0RCEb{3) 

REAL  EUx, EUy, EUz 

REAL  AGVLpx, AGVLpy , AGVLpz 

REAL  RMASS, INERT  p ( 3 ) 

LOGICAL  GYROSCOPIC 

GYROSCOPIC=.TRUE.  !  GYROSCOPIC  AFFECTS? 


DETERMINE  ANGULAR  ACCELERATIONS  IN  PLATFORM  CORDINATES 


+ 

.+ 


+ 

+ 


+ 

+ 


IF(GYROSCOPIC)THEN 
ANGACp(l.l)  = 

(TORQUp( l)+( - 

AGVLpy*AGVLp 

ANGACpf 1,2)  = 

'2  +' 

G 

3 

(TORQUpi 
AGVLpx*A! 


(TORQUp( l)+( INERTp{2) -INERTp(3) )* 
AGVLpy*AGVLpz )  /  INERTp(l) 

INERT  p ( 1 ) ) * 


(TORQUp( 2)+( INERTp 
AGVLpz*AGVLpx )  / 

ANGACp( 1,3}  = 


(Il!lERTp(2) 


3 ) + ( I NERTp ( I ) - INERT p ( 2 ) ) * 
GVLpx*AGVLpy)  /  INERTp(3) 


ELSE 

* 

00  521  IC0HP=1 ,3 

ANGACp(l.ICOMP)  =  TORQUp{ ICOMP)/INERTp( ICOMP) 
521  CONTINUE 


ENDIF 

* 

DO  522  IC0MP=1,3 

TRACCb(l, ICOMP)  =  FORCEbf ICOMP J/RMASS 
522  CONTINUE 
* 

RETURN 

END 

★ 

****★**★**★*★■*  ************  *  *********************  *********************  ***** 

*****************  *****************  ★★*★*★**  ***********************‘**********‘***** 
****************  *****************  ************************************************* 

SUBROUTINE  EULERRATE ( EUxD, EUyD, EUxD, EUx, EUy , EUz 
+  , AGVLpx, AGVLpy, AGVLpz) 

This  subroutine  uses  the  Euler  Rate  Transformation. 

It  converts  the  Angular  Rates  of  the  platform  (Body/Local) 
to  Euler  Angle  Rates.  Based  from  page  382  "Principles  of 
Dynamics"  Donald  Greenwood  and  also  Contraves  notes. 


REAL*8  EUx, EUy, EUz 

INPUT:  EUx, EUy, EUz  (Euler  Angles) 

AGVLpx, AGVLpy, AGVLpz  (Angular  Vel 

PLATFORM-BODY) 

OUTPUT:  EUxD, EUyD, EUzD  (Euler  Angular  Rates) 


r 
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* 


m. 


t 


EUxO  =  4GVLpx  +  AGVL-.y*nTAN(E"y)*DSIN(EUx)  + 

AGVLpz*DTAN(EUy )*uCoj(EUx) 

EUyO  =  AGVLpy*DCOS(EUx)  -  AGVLpz*DSIN( EUx) 

EUzD  =  AGVLpy*DSIN(EUx)/DCOS(EUy)  + 

\GVLpz 


EUzD  =  AGVLpy*DS I N ( EUx ) /DCOS ( Euy )  + 
AGVLpz*DCOS(EUx)/D 


/DCOS(EUy) 


RETURN 

END 

SUBROUTINE  EULERPARAM(eO , e 1 , e2 , e3 , EUx , EUy , EUz ) 

★ 

*  This  subroutine  determines  the  Euler  Parameters  for 

*  the  orientation  of  the  platform.  This  routine  Is  based 

*  on  the  notes  on  page  476-478  of  DADS  Theoretical  Manual 

*  where  e ( 1 -4 )  is  determined  from  the  transformation  matrix 

*  (A).  This  routine  is  good  for  determining  the  absolute 

*  value  of  Euler  Parameters,  thus  the  sign  is  not  determined. 


INPUT:  EUx, EUy , EUz 
OUTPUT:  e(4) 


Euler  Angles 
Euler  Parameters 


REAL  e0,el,e2,e3, A(3,3) 

REAL  EUx,EUy,£Uz 

Get  transformation  Matrix  A 

CALL  TRANS_MAT(EUy,EUx,EUz,A) 

TrA  =  A(l,l)+A(2,2)+A(3,3)  !  tr(A) 

eO  2  =  (TrA  +  l)/4 
e0~=  $QRT(AB$(eO_2)) 

el  2  =  (  1  +  2*A( 1. 1 )  -  TrA)/4 
el~=  5QRT(ABS(el  2  ) 


c  x  c  -  i  x  ^  C.  n  \  if  1/  -  IIO//-T 

el  =  5QRT(ABS(el_2)) 

★ 

e2  2  =  (  1  +  2*A(2,2)  -  TrA)/4 
e2~=  SQRT(ABS(e2  2)) 

*  ~ 

e3  3  =  (  1  +  2*A (3,3)  -  TrA)/4 
e3~=  SQRT(ABS(e3_3) ) 

RETURN 

END 

★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★dr** 

★★Hr************************************************* 

* 

SUBROUTINE  NETFORCE ( FORCEb , TORQUp , Q1 , Q2 , Q3 , Q4 , 


Q5,Q6,EUy,EUx,EUz,ACT_UNIT 


,  P^  ATJ 


PLAT.RMASS) 


This  subroutine  determines  the  net  TORQUp  and  FORCEb 
on  the  platform  given  the  orientation,  actuator  unit 
vector  and  magnitudes  of  the  FORCEb. 


INPUT:  Q1,Q2,Q3,Q4,Q5,Q6 

ACTUNI  (For  all  6  actuators) 
EUy,  EUx  EUz  (Orientation) 
PLAT  PLAT 
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RMASS 


OUTPUT:  FORCEb 
TORQUp 


REAL  TORQUp{3) , F0RCEb(3) 

REAL  ACT  UNIT(6, 3) , EUy , EUx, EUz 

REAL  PLAT  PLAT(6,3) .FORCE  ACT  VECT(6,3) 

REAL  FORCF  PLAT{6, 3) , TOR  ffCT(5,3),RMAS$ 


********  ACTUATOR  FORCEb  Q1,Q2,Q3,Q4,Q5,Q6  ************************* 
★ 

* 

*  CREATE  FORCE  VECTORS  FORCE  ACT  VECT(  IACT,  (  X,Y,  or  Z  )  ) 

*  1-6,  1,2,  or  3) 

*  FORCEb  IN  BASE  CORDINATES 

* 

DO  413  ICOMP=l ,3 

FORCE  ACT  VECT{ 1, ICOMP)=Ql*ACT  UNITf 1, ICOMP) 

FORCE“ACT  VECT  2, ICOMP  =Q2*ACT-UNIT  2, ICOMP) 
FORCE~ACT"VECT  3, ICOMP  =Q3*ACT"UNIT (3, ICOMP 
FORCE~ACT“VECT  4, ICOMP  =Q4*ACT~UNIT  4, ICOMP) 
FORCE"ACT"VECT  5,1  COMP  =Q5*ACT-UNIT ( 5 , ICOMP 
F0RCE-ACT-VECT(6, ICOMP  =Q6*ACT-UNIT(6, ICOMP 
413  CONTINUE  "  " 

•kfcirk  ...  .*  .  .  .  5 

****  CONVERT  FORCEb  IN  BASE  CORDINATES  TO  PLATFORM  CORDINATES 


FORCEb: 


BASE  CORD. 
PLATFORM  CORD. 


FORCE  ACT  VECTflACT,  1,2  or  3) 
FORCE”PLAT  (IACT,  1,2  or  3) 


DO  431  IACT=1,6 

CALL  REVERSETRANS( IACT, EUy, EUx, EUz, 

FORCE  ACT  V 

CONTINUE  “  " 


CE_ACT_VECT,FORCE_PLAT) 


ACCUMULATE  THE  FORCEb  AND  TORQUp  IN  PLATFORM  CORDINATES 


DO  417  ICOMP=l,3 

FORCEbf ICOMP)=0. 
TORQUpi I  COMP ) =0 . 
CONTINUE 


!  INITIAL  PREVIOUS  ARRAYS  TO  ZERO 


DO  414  IACT=1,6  !  FORCEb  IN  BASE  CORD. 
FORCEb? l)=FORCEb( 1)+F0RCE  ACT  VECT(IACT,1 
FORCEb  2  =FORCEb  2  +FORCE“ACT~VECT  I ACT , 2 
FORCEb  3)=FORCEb  3)+F0RCE"ACT~VECT  IACT, 3 


!  FORCEb  IN  X 
I  FORCEb  IN  Y 
!  FORCEb  IN  Z 


_  DETERMINE  PLATFORM  TORQUE  FROM  EACH  ACTUATOR  IN  PLATFROM  CORD. 
CALL  CROSS ( IACT, PLAT_PLAT, FORCE_PLAT,TOR_ACT) 


TORQUp ( l)=TORQUp( 1) 
TORQUp ( 2 )=TORQUp ( 2 ] 
TORQUp(3)=TORQUp(3 j 

CONTINUE 


+  TOR  ACT  IACT, 1 
+  TOR-ACT(IACT,2 
+  TOR- ACT  IACT, 3 


I  TORQUp  IN  X 
I  TORQUp  IN  Y 
!  TORQUp  IN  Z 


F0RCEb(3)=F0RCEb(3) -RMASS*32.2*12. 
RETURN 


LI'IU 

*************************************  ••  *-..********************************** 

******  g^D  OF  SUBROUTINE  SECTION  ****************************************** 
*************************************************************************** 
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APPENDIX  E 

TMBS  COORDINATE  CONFIGURATIONS 


r 


E-l 


E-2 


The  following  pages  show  the  various  TMBS  coordinate  configurations  known  from 
various  studies . 


TMBS  COORDINATE  CONFIGURATION  #1 

Was  used  in  the  study  presented  in  this  report  and  in  early  analysis  of  the 
TMBS . 


TMBS  COORDINATE  CONFIGURATION  #2 

This  coordinate  configuration  was  used  by  Contraves  Goertz  Corporation  in  the 
study  of  the  eigen  values  of  the  D  matrix.  Incorporating  this  configuration 
in  the  kinematic  study  presented  in  this  report  resulted  in  complete  agreement 
with  reference  6. 


TMBS  COORDINATE  CONFIGURATION  #3 

This  coordinate  configuration  is  suggested  for  future  use  in  the  software  which 
will  drive  the  TMBS. 


Incorporating  the  various  coordinate  configurations  in  the  kinematic  study 
presented  in  this  report  consist  of  making  the  appropriate  vector  geometry 
descriptions  in  subroutine  CONFIG  and  also  the  appropriate  sign  convention  on 
the  weight  vector  in  subroutine  NETFORCE.  The  remaining  FORTRAN  programs  should 
work  accordingly. 


DISTRIBUTION  LIST 


Copies 


Commander 

U.  S.  Army  Tank -Automotive  Command 
ATTN:  ASQNC-TAC-DIT  (Technical  Library) 

AMSTA-CF  (Dr.  Oscar) 

AMSTA-CR  (Mr.  Wheelock) 

AMSTA-R  (Mr.  Farkas) 

m  AMSTA-RR 

AMSTA-RTS  (Dr.  Hoogterp) 

AMSTA-RV  (Mr.  Sarna) 

AMSTA-RY 

AMSTA-TB 

'  AMSTA-U 

AMSTA-ZE 

Warren,  MI  48397-5000 
Commander 

Defense  Technical  Information  Center 
Bldg.  5,  Cameron  Station 
ATTN:  DDAC 

Alexandria,  VA  22304-9990 
Manager 

Defense  Logistics  Studies  Information  Exchange 

ATTN:  AMXMC-D 

Fort  Lee,  VA  23801-6044 

Commander 

U.  S.  Army  Foreign  Science  &  Tech  Center 
220  Seventh  Street  NE 

ATTN:  AIAST-RA  (Research  and  Analysis  Dir.) 
Charlottesville,  VA  22901-5396 

U.  S.  Army  Laboratory  Command 
Army  Research  Office 
P.  0.  Box  12211 

ATTN:  SLCRO-EG  (Engineering  Division) 

SLCRO-TS  (Library  Services) 

Research  Triangle  Park,  NC  27709-2211 

Director 

Harry  Diamond  Laboratories 

t  ATTN:  SLCHD-IT  (Eng.  &  Tech  Support  Div.) 

SLCHD-TA  (Tech.  Applications  Div.) 

2800  Powder  Mill  Road 
.  Adelphi,  MD  20783-1197 

* 

Commander 

U.  S.  Army  Materiel  Command 

ATTN:  AMCDE  (Development,  Eng,  &  Acqusition) 
AMCDMA-ML  (Library) 

5001  Eisenhower  Avenue 
Alexandria,  VA  22333-001 

Director 

CECOM  Research  Development  &  Engineering  Center 
ATTN:  AMSEL-RD  (Director) 

Fort  Monmouth,  NJ  07703-5001 
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Commander 

U.  S.  Army  Natick 

Research,  Development,  and  Engineering  Center 
ATTN:  STRNC-ML  (Technical  Library) 

Natick,  MA  01760-5000 

HQDA 

Office  of  Dep  Chief  of  Staff  for  Rsch  Dev  &  Acquisition 
ATTN:  ARZ-A  (Dr.  Lasser  -  Dir.  of  Army  Research) 
DAMA-AR 

Washington,  D.  C.  20310 
Director 

U.  S.  Army  Materiel  Systems  Analysis  Agency 
ATTN:  AMXSY-DD 

AMXSY-C  (Mr.  Harold  Burke) 

AMXSY-CM  (Mr.  Fordyce) 

AMXSY-MP  (Mr.  Cohen) 

Aberdean  Proving  Grounds,  MD  21005-5071 
Director 

Keweenaw  Research  Center 
Michigan  Technological  University 
Houghton,  MI  49931 

Director 

TRADOC  Systems  Analysis  Activity 
ATTN:  ATOR-TF 

White  Sands  Missile  Range,  MN  88002-5502 

General  Dynamics  Land  Systems  Division 
Analytical  Engineering 
ATTN:  R.  J.  Thompson 
ATTN:  Glenn  Socks 
ATTN:  J.  B.  Jovi 

MZ  436-21-19 
P.O.  Box  2045 
Warren,  MI  48090 

TRW  Steering  &  Suspension  Division 
Research  &  Development 

ATTN:  Dr.  Dave  J.  D'Onofrio 
ATTN:  James  Page 
34201  Van  Dyke  Ave. 

Sterling  Heights,  MI  48077 

Duke  University 
Mechanical  Engineering 
ATTN:  Dr.  D.  P.  Garg 
Durham,  NC  27706 

Contraves  Goerz  Corporation 
ATTN:  TMBS  Manager  (P.  Faller) 

610  Epsilon  Drive 
Pittsburg,  PA  15238 

University  of  Michigan 
Transportation  Research  Institute 
ATTN:  Library  (Ann  Grimm) 

2901  Baxter  Road 

Ann  Arbor,  MI  48109-2150 


